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Abstract 


Inertial  Navigation  System  (INS)  aiding  using  bearing  measurements  taken  over  time 
of  stationary  ground  features  is  investigated.  A  cross  country  flight,  in  two  and  three 
dimensional  space,  is  considered,  as  well  as  a  vertical  drop  in  three  dimensional  space. 
The  objective  is  to  quantify  the  temporal  development  of  the  uncertainty  in  the  navigation 
states  of  an  aircraft  INS  which  is  aided  by  taking  bearing  measurements  of  ground  objects 
which  have  been  geolocated  using  ownship  position.  It  is  shown  that  during  wings  level 
flight  at  constant  speed  and  a  fixed  altitude,  an  aircraft  that  tracks  ground  objects  and  over 
time  sequentially  transitions  to  tracking  new  ground  objects  which  were  geolocated  by  the 
aircraft  as  they  came  into  view,  will  have  the  beneficial  effect  of  considerably  reducing  the 
long  term  uncertainty  in  the  INS -provided  navigation  state.  It  is  also  shown  that  a 
munition  in  “free  fall”  tracking  previously  geolocated  ground  features  will  also  have  the 
beneficial  effect  of  reducing  the  uncertainty  in  the  INS-provided  navigation  state. 
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1  Introduction 


1.1  Background 

There  are  many  techniques  to  provide  navigation  information.  They  range  from  an 
individual  standing  on  the  ground  holding  a  map  and  looking  at  his  surroundings  to 
modern  military  aircraft  with  high  quality  Inertial  Navigation  Systems  (INS)  with  Global 
Positioning  System  (GPS)  providing  very  precise  position  and  velocity  updates.  The  use 
of  GPS  is  very  prolific  in  military  operations.  However,  General  Schwartz,  the  Chief  of 
Staff  for  the  United  States  Air  Force,  is  quoted  as  saying  [4], 

“Global  positioning  has  transformed  an  entire  universe  of  war-fighting 
capability.  Our  dependence  on  precision  navigation  in  time  will  continue  to 
grow,. . .  It  seemed  critical  to  me  that  the  joint  force  reduce  its  dependence  on 
GPS  aid. . .  Our  operations  cannot  grind  to  a  halt  for  a  degraded  or  denied 
system,. . .  We  must . . .  proceed  to  build  more  resilient  systems _ ” 

This  paper  looks  at  using  vision-aided  navigation  a  la  Simultaneous  Localization  and 
Mapping  (SLAM)  to  provide  a  robust  navigation  solution  in  environments  where  GPS 
may  be  denied. 

1.2  Motivation 

Consider  a  Low  Observable  (LO)  aircraft  (A/C)  carrying  a  LO  munition  on  a  mission 
into  enemy  territory  to  eliminate  a  relocatable  High  Value  Target  (HVT).  During  the 
mission  briefing  the  location  of  the  HVT  was  limited  to  a  small  area.  The  adversary 
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actively  uses  anti-GPS  technologies,  thus  denying  the  precision  and  accuracy  of  GPS.  The 
aircraft  has  a  navigation  quality  INS,  but  the  length  of  the  flight  is  long  enough  that  the 
errors  produced  by  the  INS  are  too  large  for  a  targeting  solution.  The  pilot  needs  a  better 
navigation  solution,  but  does  not  want  to  inform  the  enemy  of  his  aircraft’s  presence  by 
using  active  navigation  techniques,  such  as  radar.  The  pilot  needs  a  better  navigation 
estimate  than  the  INS  alone  can  provide,  but  he  also  needs  to  maintain  the  autonomy 
provided  by  the  INS. 

Enter  vision  aided  navigation.  The  aircraft  uses  its  INS  estimate  to  geolocate  ground 
features,  track  those  features  to  aid  the  INS,  and  using  that  aided  estimate  geolocate  new 
features  as  the  original  features  leave  the  camera  Field  of  View  (FOV).  This  aiding 
scheme  constrains  the  error  enough  to  obtain  target  solution.  The  munition,  with  its  lower 
quality  INS,  uses  a  similar  visual  scheme.  It  looks  within  the  area  given  during  the 
mission  briefing  for  the  HVT.  Once  it  geolocates  the  HVT,  it  also  geolocates  and  tracks 
multiple  ground  features  as  it  falls.  The  munition  impacts  the  HVT  and  the  aircraft  begins 
the  return  trip  home  without  emitting  any  signal  giving  away  the  its  location. 

1.3  Scope 

There  are  several  important  aspects  of  visual  navigation  that  will  not  be  discussed  in 
this  paper.  They  include  image  processing,  to  include  autonomous,  without  human 
assistance,  feature  detection  and  feature  correspondence.  It  is  assumed  that  autonomous 
feature  detection  and  tracking  are  possible,  such  as  by  the  SIFT  image  processing 
algorithm  [5]  or  by  the  stochastic  process  set  forth  in  [10]. 

This  paper  focuses  exclusively  on  gauging  the  performance  of  an  inertial  navigation 
system  aided  by  a  vision  based  SEAM  during 

1 .  A  horizontal  flight  in  two  dimensional  space 

2.  A  horizontal  flight  in  three  dimensional  space 
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3.  A  vertical  drop  in  three  dimensional  space 


The  measure  of  performance  will  be  the  uncertainty,  or  standard  deviation,  of  the 
navigation  states  provided  by  the  aided  INS.  These  values  will  be  compared  to  the 
uncertainty  of  the  navigation  states  in  the  free  INS. 

1.4  Approach/Methodology 

To  document  this  research,  each  chapter  will  begin  with  a  brief  overview  of  the  topics 
discussed  in  the  chapter.  Chapter  2  provides  background  information  for  the  INS,  the 
reference  frames  used  when  working  with  an  INS,  a  brief  discussion  of  SLAM  and 
concludes  with  recent,  relevant  research.  Chapter  3  shows  the  mathematical  development 
of  the  2-D  case,  including  the  dynamics  and  measurement  model  development,  the  state 
space  representation  and  the  use  of  the  Kalman  filter  mechanization.  This  information  was 
then  extended  to  look  at  the  3-D  case  for  both  a  horizontal  flight  and  a  vertical  fall. 
Chapter  4  looks  at  the  results  of  the  covariance  analysis.  Finally,  Chapter  5  summarizes 
the  key  points  of  the  paper,  focusing  on  the  impact  the  INS  aiding  scheme  provided.  A  list 
of  potential  follow-on  research  topics  is  also  provided. 
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2  Literature  Review 


2.1  Overview 

This  chapter  provides  foundational  information  regarding  the  topics  discussed  in  the 
following  chapter.  Section  2.2  discusses  the  different  coordinate  frames  of  reference  used 
in  this  paper.  Section  2.3  discusses  the  fundamentals  of  inertial  navigation,  including  a 
brief  discussion  of  the  sensors  used  and  how  they  work.  Section  2.4  discusses  SLAM. 
Finally,  Section  2.5  reviews  recent  research  that  contributed  to  this  paper. 

2.2  Reference  Frames 

2.2.1  Earth-fixed  reference  frame.  The  Earth-fixed  reference  frame  has  its  origin 
fixed  to  an  arbitrary  point  on  the  surface  of  the  Earth.  The  axis  XE  points  due  North,  Y E 
points  due  east  and  ZE  points  to  the  gravitational  center  of  the  Earth.  This  reference  frame 
is  shown  in  Figure  2.1. 

2.2.2  Navigation  reference  frame.  The  navigation  frame  is  a  modified  version  of 
the  Earth-fixed  reference  frame.  The  origin  of  this  reference  frame  is  also  placed 
arbitrarily  on  the  surface  of  the  Earth,  where  z  =  0  at  the  surface  of  the  earth.  The  only 
difference  between  the  navigation  frame  and  the  Earth-fixed  frame  is  that  the  axis  are 
rotated  180°  about  the  X  axis.  Therefore  Yn  points  due  west  and  Z„  points  exactly  away 
from  the  center  of  the  Earth.  This  reference  frame  is  shown  in  Figure  2.2. 

2.2.3  Body-fixed  reference  frame.  This  reference  frame  has  its  origin  located 
somewhere  on  an  A/C.  Normally  the  origin  is  set  at  the  A/C  center  of  gravity  to  allow  the 
use  of  the  equations  of  motion.  However,  as  fuel  is  used  during  flight,  the  center  of  gravity 
does  not  remain  constant.  Therefore  in  this  research,  the  origin  is  set  at  the  origin  of  the 
camera.  The  fact  that  it  is  “body-fixed”  indicates  that  the  axis  do  not  change  with  respect 
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Figure  2.1:  Earth-fixed  reference  frame 


to  the  A/C  body  as  A/C’s  trajectory  changes.  Rather,  the  Xb  axis  is  the  longitudinal  axis 
and  typically  runs  through  the  nose  of  the  A/C,  the  Yb  axis  is  the  lateral  axis  and  runs 
parallel  with  the  left  wing,  and  the  Zh  axis  is  the  vertical  axis  and  runs  out  the  top  of  the 
A/C.  This  reference  frame  is  shown  on  a  F-35  Lightning  II  in  Figure  2.3 

2.3  Inertial  Navigation 

The  INS  provides  the  navigation  state  based  on  perceived  changes  in  the  inertia  and 
moment  of  inertia  of  the  sensors  utilized.  These  changes  are  quantified  based  on  Newton’s 
mechanical  laws.  INSs  are  used  in  many  vehicle  types,  including  air,  sea,  ground  and 
space,  but  the  focus  in  this  research  will  be  on  air  based  vehicles.  An  INS  employs  a  suite 
of  sensors,  typically  three  accelerometers  and  three  gyroscopes,  one  for  each  of  the  body 
axes,  and  an  onboard  computer  to  compile  and  integrate  the  information  provided  by  the 
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Figure  2.2:  Navigation  reference  frame 


sensors.  This  integrated  data  provides  the  operator  of  the  A/C  with  an  estimate  of  the 
current  navigation  state  based  on  the  initial  navigation  state. 

An  INS  is  initialized  prior  to  take  off  using  external  sources,  i.e.,  GPS  for  position, 
pilot  setting  velocity  to  zero,  and  runway  markers  and  levels  for  the  Euler  angles.  This 
initialization  is  critical  because  all  later  estimates  build  upon  the  initial  values.  If  the 
starting  point  for  the  INS  is  incorrect,  it  will  only  get  worse  as  time  goes  on.  Once 
initialized,  the  INS  can  autonomously  provide  a  navigation  state  estimate  without  any 
external  assistance,  thus  it  is  unaffected  by  adversary  jamming  or  atmospheric 
interference. 

The  onboard  accelerometers  indicate  translational  changes  in  an  A/C’s  position. 
They  sense  acceleration  along  the  A/C’s  axes.  The  detected  acceleration  is  integrated  to 
calculate  the  A/C’s  velocity,  which  in  turn  is  integrated  to  provide  position.  However, 
there  is  no  orientation  data.  As  such,  if  there  were  no  other  sensors,  the  A/C  would  only 
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Figure  2.3:  Body-fixed  reference  frame  shown  on  F-35  Lightning  II 


know  how  much  it  moved  left  or  right,  forward  or  backward,  and  up  or  down  relative  to 
itself.  It  would  be  akin  to  being  blindfolded,  and  placed  in  a  car.  One  would  feel 
acceleration  forward  or  backward  by  being  pressed  into  the  seat  or  against  the  seatbelt, 
respectively. 

Conversely,  the  gyroscopes  indicate  rotation  rate.  Then  based  on  the  initialized 
values,  the  rotation  rate  is  integrated  to  determine  the  A/C’s  angles.  This  provides  the 
orientation  data  that  the  accelerometers  could  not  provide.  However,  the  gyroscopes 
cannot  detect  the  magnitude  of  the  translation  vector.  This  would  be  like  being  in  a  car 
blindfolded.  One  would  feel  the  turns,  and  would  know  the  direction  of  the  turn  but  not 
how  fast  the  car  was  traveling  outside  of  the  turn. 

Both  sensors,  when  operating  independently,  offer  valuable,  if  incomplete 
information.  It  is  when  the  information  from  both  are  brought  together  that  a  strong 
navigation  state  estimate  is  provided  to  the  user.  The  translational  information  combined 
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with  the  orientation  information  takes  the  data  from  the  body-fixed  reference  frame  and 
makes  it  useful  by  putting  it  in  the  navigation,  or  inertial,  reference  frame.  For  example, 
the  sensed  movements  left  or  right  would  translate  to  calculated  movement  North  or  South 
from  the  starting  location,  based  on  the  orientation  of  the  A/C. 

The  autonomy  of  the  INS  does  have  a  major  limitation:  the  sensors  are  prone  to  drift 
and  noise.  These  sensor  errors  accumulate  over  time  as  the  readings  from  the  gyroscopes, 
to  include  the  errors,  are  integrated  into  the  angles.  The  accelerometers  provide  a  double 
dose  of  the  errors  because  they  are  integrated  twice  to  calculate  the  position  estimate. 
These  errors  cause  the  INS  navigation  state  estimate  to  drift  over  time,  with  no  upper  limit 
to  the  estimate  error.  The  only  way  to  constrain  the  error  is  through  an  external 
measurement.  Common  examples  of  measurements  used  are  barometric  altimeters,  GPS, 
doppler,  and,  more  recently,  visual  bearings-only  measurements. 

2.4  SLAM 

SLAM  is  a  growing  topic  of  interest  in  the  robotics  community.  It  is  commonly 
believed  that  the  ability  for  a  robot  to  discern  its  environment  and,  at  the  same  time, 
determine  its  position  within  the  environment  is  a  crucial  step  to  true  robotic  autonomy 
[2].  The  fundamental  issue  with  SLAM  is  shown  in  Figure  2.4  [2]. 

The  typical  scenario  is  that  a  robot,  with  some  sort  of  onboard  sensor,  i.e.,  laser  range 
finder,  camera,  sonar  etc.,  is  placed  in  an  environment  with  no  a  priori  information 
regarding  the  environment.  The  robot  then  begins  to  probabilistically  estimate  its  own 
position  and  that  of  the  ground  features  around  it,  typically  with  some  flavor  of  a  Kalman 
filter.  Because  the  features  in  the  environment  are  being  estimated,  each  feature  adds  some 
number  of  states  to  the  state  vector.  The  number  depends  on  the  type  of  information  being 
used.  For  example,  in  a  2-d  case,  if  the  position  of  the  ground  feature  is  being  estimated, 
and  the  feature  is  stationary,  two  states  would  have  been  to  be  concatenated  on  the  end  of 
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Figure  2.4:  This  shows  fundamental  problem  of  how  the  vehicle  must  estimate  its  own 
position  at  the  same  time  that  it  is  estimating  the  position  of  the  features  used  for  tracking. 
Neither  position  is  ever  truly  known[2]. 


the  state  vector.  In  a  3-d  case,  for  a  moving  feature,  six  states  would  have  to  be  added  to 
the  state  vector.  This  has  potential  to  create  a  very  large  state  vector  quickly,  which, 
depending  on  the  filtering  method  used,  can  become  computationally  burdensome  [2]. 

Feature  recognition  and  tracking  is  crucial  to  SLAM.  If  the  robot  cannot  track  or 
correlate  features  from  one  update  to  the  next,  it  will  not  be  able  to  build  a  viable  map.  At 
best  it  would  have  a  series  of  independent  measurements  with  no  way  of  connecting  the 
dots  to  see  the  big  picture.  It  is  this  aspect  of  the  problem  that  has  been  the  truly  limiting 
factor  for  SLAM. 

2.5  Recent  Research 

In  [8],  Pachter  et.  al  explored  the  concept  of  using  bearings-only  visual 
measurements  to  aid  an  INS  in  three  dimensions.  It  was  theoretical  work,  meaning  there 
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was  no  simulation  or  empirical  data  substantiate  the  theory.  It  did  however  develop  the 
mathematics  for  observing  the  angle  to  a  ground  feature  over  time  and  using  that 
information  to  update  an  INS,  thus  constraining  the  error. 

In  [3],  Giebner  used  visual  measurements  to  aid  an  A/C  INS,  via  Kalman  filter,  while 
flying  a  circular  orbit  around  a  single  ground  feature,  both  in  simulation  and  in  an  actual 
test  environment.  He  found  that  using  visual-measurements  reduced  the  uncertainty  of  the 
position  of  the  A/C,  in  three  dimensions  after  6  minutes  of  flight  time,  from  350  meters,  in 
the  unaided  INS  case,  down  to  50  meters  when  combined  with  barometric  altimeter 
readings. 

In  [7]  observability  of  a  vision-aided  INS  was  explored.  The  measurements  were  time 
dependent  due  to  the  fact  that  the  position  of  the  ground  feature(s)  being  tracked,  relative 
to  the  A/C,  changed  with  time.  Because  the  measurement  matrix  was  time  dependent,  the 
observability  Grammian  was  used.  It  was  found  that  when  the  INS  was  only  aided  by 
bearings-only  visual  measurements  of  a  single  ground  feature,  the  observability 
Grammian  was  rank  deficient,  thereby  indicating  incomplete  INS  aiding  action.  However, 
if  a  second  ground  feature  was  simultaneously  tracked,  the  observability  Grammian  had 
full  rank,  thus  there  was  complete  INS  aiding  action;  that  is  that  all  of  the  navigation  states 
received  some  improvement  from  the  measurement  when  compared  to  the  unaided  case. 

In  [2]  a  detailed  history  of  SLAM  was  provided.  It  also  provided  fundamental 
information  regarding  implementing  SLAM  with  various  filter  methods.  It  showed  that 
the  uncertainty  of  detected  features,  as  well  as  the  navigation  estimate,  will  decrease  as 
more  measurements  are  taken.  An  example  was  provided  where  a  robot  was  piloted 
remotely  through  an  indoor  environment.  The  pilot  did  not  have  visual  access  to  the  robot. 
Instead,  the  pilot  navigated  based  on  the  map  rendered  by  the  robot.  The  robot  then 
autonomously  returned  to  the  starting  point.  Other  examples  were  provided  as  well. 
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In  [1]  an  algorithm  for  single  a  camera  to  perform  SLAM  in  real  time,  called 
MonoSLAM,  was  developed.  The  authors  were  concerned  with  mapping  and  localizing  a 
small  area.  They  found  that  even  in  a  feature  sparse  environment  they  could  build  a  map 
of  points  and  eliminate  drift  once  the  robot  began  tracking  a  previously  tracked  feature. 
MonoSLAM  was  then  used  on  a  humanoid  robot  fitted  with  three  cameras  walking  in  a 
circle.  They  found,  until  the  robot  tracked  a  previously  tracked  feature,  the  feature 
uncertainty  continued  to  grow.  However,  once  features  came  back  into  view,  the 
uncertainty  stopped  growing.  They  concluded  that  a  robot  would  be  able  to  repeat  the  task 
indefinitely  without  any  drift  in  the  localization  accuracy. 
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3  Methodology 


3.1  Introduction 

This  chapter  is  broken  up  into  three  main  sections.  Section  3.2  discusses  the 
two-dimensional  case,  including  the  error  dynamics  model  development,  calibrating  the 
unaided  INS,  measurement  model  development  and  how  the  calculations  were 
accomplished.  Section  3.3  will  cover  the  same  topics  as  Section  3.2,  but  for  the 
three-dimensional  case  where  the  A/C  is  flying  wings  level,  constant  altitude  with  its 
longitudinal  axis  aligned  with  the  X„  axis  and  small  angle  assumptions  are  valid  for  the 
A/C’s  Euler  angles.  The  Section  3.4  will  expand  on  Section  3.3  for  the  vertical  free  fall 
case  where  all  the  body  axes  are  still  aligned  with  the  navigation  axes.  Development  of  the 
general  form,  where  small  angle  assumptions  are  not  used  on  the  A/C’s  Euler  angles,  of 
the  dynamics  model  is  shown  in  Appendix  D,  and  the  development  of  the  general  form  of 
the  measurement  model  is  shown  in  Appendix  E. 

3.2  Two-Dimensional  Case 

The  two-dimensional  case  is  the  simpler  of  the  two  cases,  and,  as  such,  is  the 
advantageous  place  to  start.  This  research  will  start  with  the  development  of  the  dynamics 
model,  which  is  the  error  state  equations. 


3.2. 1  Dynamics  Error  Model.  The  navigation  frame  is  the  “inertial”  (Xn,Zn)  frame 
and  the  A/C  body  axes  are  (Xb,  Zh).  The  A/C’s  position  is  (x,  z)  and  6  is  the  pitch  angle.  A 
strapdown  [9]  INS  arrangement  is  considered.  When  flying  in  the  vertical  plane  of  a 
non-rotating  flat  Earth  as  shown  in  Figure  3.1,  the  reference  frames  can  be  related  by  the 
Direction  Cosine  Matrix  (DCM),  C/  [9]: 


c6 

s  e 

-s  e 

cd 

(3.1) 
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Figure  3.1:  The  A/C  in  the  vertical  plane.  Notice  that  the  origin  of  the  body  frame  is  at  the 
camera. 


The  specific  force,  f  measured  by  an  accelerometer  is 


f  =  a-g 


(3.2) 


where  a  is  the  A/Cs  acceleration  and  g  is  the  Earth’s  gravitational  force.  The  specific  force 
resolved  in  the  inertial  frame  is 


'  A"' ' 

'  /f 

_ 

—  '-'fa 

A”\ 

.  f?  _ 

where  the  superscript  indicates  which  frame  of  reference  is  being  used, (n)  for  the  inertial 
navigation  frame  frame  and  (h)  for  the  body  frame,  fxh)  and  f-b)  are  the  specific  forces 
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measured  by  the  longitudinal  and  vertical  accelerometers,  respectively,  and  6  is  the  pitch 
angle.  In  an  effort  to  conserve  space,  which  is  necessary  in  subsequent  sections,  cos  (★) 
and  sin  (★)  will  be  abbreviated  with  c*  and  s ★  respectively,  where  ★  can  be  any  angle. 
The  specific  forces  are  fed  into  the  states  by 

V?  =  if  +  s? 

yin)  _  An)  An) 

'  Z  Jz  '  oz 

and 

V*0  =  i(») 

yin)  — 

OJ  =  0 


where  V  is  the  acceleration  of  the  A/C  with  the  subscripts  x  and  ,  indicating  which 
direction.  The  A/C’s  acceleration  is  integrated  to  determine  its  x  and  z  velocity,  V,  which 
produces  the  respective  positions  v  and  z  by  a  second  integration. 

However,  the  sensors  have  uncertainty,  so  these  equations  must  be  perturbed  using  a 
first  order  Taylor  series  expansion.  This  produces 

Sf(Kn)  =  (-/f }  sin  6  +  f(7b)  cos  6)86  +  8f(xb)  cos  6  +  8f)b)  sin  6 

(3.3 

=  fzn)S6  +  8f(b)  cos  6  +  Sf^  sin  6 

and 


Sff  =  {-f(b)  cos  6  -  sin  6)86  -  8f(b)  sin  6  +  8fb)  cos  6 
=  -f(f86  -  8f(b)  sin  6  +  8fb)  cos  6 


(3.4) 


14 


where  Sfxb)  and  8ff)}  are  the  errors  of  the  accelerometers.  Thus  the  error  equations  are 
formed 


8x(n>  =  5V™ 
8z(n)  =  8V[n) 
8V(xn>  =  8ftn) 
c W'f  =  8f^n) 


86  =  8co 


and  substituting  Eqs.  (3.3)-(3.4)  into  Eqs.  (3.7)-(3.8)  yields 

SV(f  =  f^86  +  8fxb)  cos  6  +  8f(b)  sin  6 
SVf  =  -ffS6  -  8fxb)  sin  6  +  6f±b)  cos  6 


(3.5) 

(3.6) 

(3.7) 

(3.8) 

(3.9) 


(3.10) 

(3.11) 


Eqs.  (3.5),  (3.6),  (3.9),  (3.10)  and  (3.11)  are  the  INS  error  equations  when  flying  wings 
level  over  a  flat  and  non-rotating  Earth.  The  error  equations  are  shown  in  state  space 
notation  as,  <5x  =  Adx  +  Tdu,  where  A  is  the  error  state  dynamics  matrix,  the  navigation 
state’s  error  vector,  8x  is 


=  [  8x  Sz  8VX  8VZ  86  f  (3.12) 

and  the  disturbances,  ciu.  are  the  two  accelerometers’  and  the  rate  gyroscope’s  biases 

du  =  [  8f(b)  8f7h)  8(0  f  (3.13) 

For  this  case  the  trajectory  is  assumed  to  be  a  wings  level,  constant  altitude  flight, 
therefore  the  nominal  variables  are  6  =  0,  fin)  =  g  and  f(x"'  =  a,  where  g  is  the  acceleration 
of  gravity  and  a  is  the  longitudinal  acceleration  of  the  A/C.  These  variables  are 
non-dimensionalized  in  order  to  maintain  geometry  and  remove  any  issues  where  the  units 
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may  not  coincide.  They  are  nondimensionalized  as  follows 
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v 
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where  t  is  the  current  time,  7'  is  the  duration  of  a  measurement  epoch  and  /?  is  the  above 
ground  level  altitude.  The  non-dimensional  parameters  are 

ha 


a  hg 

8  =  — 
v2 


and 


A 

a 


1 1  ,  for  example, 

h  =  1000[m], 


v  =  100 


777 


L  sec 


8  =  10 


777 


L  see¬ 


the  non-dimensional  parameter  g  =  1.  Therefore,  the  INS  error  state  equations  are 


6x  = 


0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

8 

dx  + 

1 

0 

0 

0 

0 

0 

0 

-a 

0 

1 

0 

0 

0 

0 

0 

0 

0 

0 

1 

du 


(3.14) 


and  with  perfect  INS  alignment 


dx(0)  =  0,  0  <t<T 


The  nondimensional  duration  of  a  measurement  session,  or  epoch,  is  T  =  1. 

It  is  assumed  that  the  sensor  errors  are  Gaussian  distributed,  constant  biases.  This 
allows  the  state  error  vector  to  be  augmented  with  the  vector  u;  the  augmented  state  is 


dxa 


dx 


du 


8x1 


(3.15) 
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and  the  A  matrix  is  augmented  by  the  F  matrix,  as  shown 

a  r 

Aa  = 

03x5  03X3 

When  converted  to  discrete  time  Aa  — >  Aad  = 
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where  At  is  the  sampling  period.  The  discrete  time  state  dynamics  become 


(3.16) 


(3.17) 


Sxa(l  +  1)  =  Aaddxa(/),  /  =  0, . . . ,  N  -  1  (3.18) 


where  l  is  is  the  discrete  time  step  counter  and  the  non-dimensional  period  between  steps 
is  AT  =  ^  =  A t^.  Setting  a  to  zero  (assume  a  constant  cruise  speed)  simplifies  the 
dynamics  to 

1  0  A/  0  f  f  0  f 

0  1  0  At  0  0^0 

0  0  1  0  ^  A(  0  f 

0  0  0  1  0  0  At  0 

Aad  = 

0  0  0  0  1  0  0  At 

0  0  0  0  0  1  0  0 

0  0  0  0  0  0  1  0 

0  0  0  0  0  0  0  1 
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This  dynamics  equation  applies  as  long  as  the  ground  features’  positions  are  known. 
Assuming  the  ground  features  are  stationary,  but  their  position  is  not  known,  an  additional 
state  must  be  added  for  each  tracked  ground  feature  whose  position  is  estimated.  In  our 
two  dimensional  scenario,  if  the  number  of  ground  features  being  tracked  is  n,  then  the 
augmented  navigation  state  is 

\  *9,  1 


dxa  :  = 


Sxpi 


(3.19) 


and 


8x 


pn 


(8+h)x1 


Aad  1  — 


Aa  Osxn 

0«x8  I hxh 


J(8+n)x(8+«) 


(3.20) 


On  one  hand,  state  augmentation  reduces  the  degree  of  observability,  which  decreases  the 
strength  of  INS  aiding  action.  On  the  other  hand,  however,  the  inclusion  of  additional 
features  to  be  tracked  helps  wash  out  the  measurement  error. 


3.2.2  Modeling/Calibrating  the  Free  INS.  With  the  dynamics  from 
Subsection  3.2.1,  the  values  for  cra  and  crg,  the  uncertainty  in  the  bias  of  the 
accelerometers  and  gyroscope,  respectively,  are  set  such  that  the  free  INS  is  a  1  ^ 
navigation  system;  a  non-dimensional  hour  is  360  non-dimensional  seconds.  There  are 
A  =  100  discrete  steps  in  a  non-dimensional  second.  The  calibration  is  performed  by 
using  the  solution  to  the  Lyapunov  difference  equation 

P(7  +  1)  =  AadP(7)Aad,  0  <  /  <  360N  -  1  (3.21) 
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with 


P(0) 


05x5  05x1  06x1  O7XI 

0ix5  cr2a  \  \ 

0lx6  •  •  •  CT2a 

0ix7  .  cr2„ 


(3.22) 


J8x8 


There  is  a  linear  relationship  between  the  uncertainty  in  the  A/C’s  x  position  and  the 
uncertainty  in  the  sensors’  biases 


PU(36(W)  =  a<r2a  +  / 3cr 2 


(3.23) 


where  the  coefficients  a  and / 3  are  constants.  Therefore,  Eq.  (3.21)  was  solved  for  one 
non-dimensional  hour  twice  to  calculate  the  values  of  the  constants.  The  first  time,  cra  was 
set  to  1  and  crg  was  set  to  0.  The  second  time,  cra  was  set  to  0  and  crg  was  set  to  1 .  Then 
assigning  the  errors  in  the  accelerometers  and  the  error  in  the  gyroscope  an  equal  role  in 
the  uncertainty  of  the  A/C’s  position,  the  values  for  the  variances  of  the  sensors’  biases 
variances  are 


£ ra  =  =  1.0912  x  10"5  (3.24) 

yjla 

<rg  =  — U  =  9.0935  x  10"8  (3.25) 

3.2.3  Measurement  Model.  Based  on  the  geometry  established  in 
Subsection  3.2.1,  the  foundational  relationship  between  the  visual  measurements  and  the 
ground  feature’s  position  is  created 


tan/;  = 


f 


(xp-x)M 
( Zp  -  z){b) 


(3.26) 


where  d  is  the  angle  between  the  longitudinal  body  axis  and  the  Line  of  Sight  (LOS) 
vector,  which  runs  from  the  origin  of  the  body  axis  to  the  ground  feature,  Xf  is  the 
projection  of  the  ground  feature’s  position  onto  the  camera’s  focal  plane,  /  is  the  focal 
length  of  the  camera,  and  xp  and  z.p  are  v  and  z  coordinates,  respectively,  of  the  ground 
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feature.  The  relationship  of  the  body  coordinates  to  the  navigation  is  shown  by  the  DCM 
Cb,  where 


cd 

-sd 

sd 

cd 

(3.27) 


thus 


r  -I 
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xp  -  X 
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(3.28) 


The  projected  position  is  nondimensionalized  such  that 


xf 


f 


Assuming  the  ground  feature  is  on  the  ground,  combined  with  the  non-dimensionalization 
shown  above,  simplifies  Eq.  (3.26)  to 

_  {Xp-x)(n)c6+{zp-z)in)  sd 
1  (Xp  -  x)(n)  S0  -( Zp  -  z)(n)  cO 

It  should  be  noted  that  for  the  remainder  of  this  subsection  all  coordinates  will  be  in  the 
navigation  frame.  As  such,  the  {n)  will  be  dropped. 

Moving  everything  to  the  Left  Hand  Side  (LHS)  yields 


Xf[(xp  -  x)(n)  cd  +(zp  -  z){n)  s0]  +  (xp  -  x)(n]  sd  -( zp  -  z){n)  cd  =  0  (3.29) 

The  INS  provides  the  “calculated”  navigation  state  [  Zc  Vxc  v_c  dcY,  where  c 
indicates  the  calculated  value  provided  by  the  INS.  However,  the  true  values  of  the  states 
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and  measurements  are  not  known.  The  following  perturbations  hold 


xc  =  x  +  8x  zc  =  z  +  Sz 

8C  =  6  +  86  xpc  =  xp  +  8xp  Xfm  =  Xf  +  8xf 

where  m  indicates  a  measured  valued.  By  way  of  the  trigonometric  addition  formula  and 
small  angle  approximation,  the  sin  and  cos  terms  simplify  to 

sin(6c  -  86)  =  s 6C  -(s 6C  +  c8c)86  cos (6C  -  86)  =  c6c  +(s 6C  -  c6c)86 

Substituting  the  perturbations  into  Equation  3.29,  yields 


(xfm-8xf)[(sdc  —(s6c  +  c6c)86)((xpc-xc)  +  8x  -  8xp)  -  (c 6C  +(s 6C  -  c6c)86)(zc  -  &)] 

-  ( c6c  +(s 6C  -  c6c)86)((xpc-xc)  +  8x  -  Sxp)  -  (s 6C  -(s 8C  +  c6c)86)(zc  -  8z)  =  0  (3.30) 

To  simplify  the  algebra,  the  LHS  will  be  broken  up  into  components  such  that 


A  comp  =  (s#c  -(S0C  +  C6c)86)((xpc-xc)  +  8x  -  8xp)  -  (c0c  +(s0c  -  c6c)86)(zc  -  8z)  (3.3 1) 

Bcomp  =  -(c 6C  +(s 6C  -  c6c)86)((xpc-xc)  +  8x  -  8xp)  -  (s 6C  -(s 6C  +  c6c)S6)(zc  -  8z)  (3.32) 


such  that 


(xf  m  8Xf)A  cnnip  +  Bcomp  0 


(3.33) 


The  terms  within  the  components  need  to  be  distributed  out.  When  two  small  errors  are 
multiplied  together,  their  product  is  considered  negligible.  As  such,  the  component  terms 
become 


Acomp  ^6q(x pc  Xc)  cdc  Zc  8x  s^c  ~^~8z  c0c 


-  86((s6c  +  c6c)(xpc-xc)  +  (s 6C  -  c6c)zc)  -  8xp  s8c 


(3.34) 


and 


Bcomp  =  (-  zc  ~  c9c(Xpc-xc))  -  8x  c6c  +8z  s 6C  +8xp  c8c 


+  86((s8c  +  c9c)zc-)  +  8xp  c8c 


(3.35) 
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Substituting  Eqs.  (3.34)-(3.35)  into  Eq.  (3.33)  and  collecting  like  error  terms  yields 

Xfm(.S@c(Xpc  Xc)  C9c  Zc )  s(9c  Zc  c9c i^XpC  Xi:)  SXj' (s9c(x  pr  Xc)  c9c  Zc') 

+  8x(s9c  Xfm  -  c9c )  +  6z(s9c  +  c9c  Xfm)  +  6xp(c9c  -  s 9C  Xfm) 

+  <S0[(s0c  +  c 9c)zc  -  (s 9C  -  c9c)(xpc-xc)  -  ((s 9C  +  c9c)(xpc-xc )  +  (s 9C  -  c 9c)zc)xfm\  =  0 

(3.36) 

Moving  the  error  terms  to  the  Right  Hand  Side  (RHS)  yields 

Xfm(*0i:(XpC  xc )  c9c  zd  §9C  Zc  c9c(xpc  x^)  8xj(s9c (x pc  xc)  c9c  Zc ) 

+  6x(c9c  -  s 9C  xfm)  -  8z(s9c  +  c9c  xfm)  -  Sxp(c9c  -  s 9C  xfm)  (3.37) 

-  d0[(s0c  +  c9c)zc  ~  (s0c  -  c9c)(xpc-xc)  -  ((s 9C  +  c9c)(xpc-xc)  +  (s 9C  -  c 9c)zc)xfm\ 

On  the  RHS,  the  nominal  value  of  the  states  and  the  measurement  are  substituted  in  for  the 
calculated  and  measured  values  such  that 

xfm(c9c  Zc  s9c(xpc  xc ) )  +  (s 9C  Zc  c9c(xpc  xc))  8xf(*9(xp  .v)  c9  z) 

+  8x(c9  —  %9 Xf)  -  Sz(s9  +  c9xf)  -  6xp(c9-s9xf)  (3.38) 

-  50[(s0  +  c9)z  -  (s 9  -  c9)(xp  -  x)  -  ((s 9  -  c9)z  +  (s 9  +  c9)(xp  -  x))xf  ] 

For  the  scenario  laid  out  at  the  beginning  of  this  section,  z  =  h,  which  gives  it  a  value  of  1 
when  nondimensionalized  according  to  Subsection  3.2.1,  9  =  0  and  Xf  =  ~(xp  -  x). 
Therefore  the  measurement  matrix,  H(/)  for  a  single  unknown  ground  feature  is 

H„(/)=  1  Xp  -  x(l)  0  0  2(xp  -  x(l))  -  (xp  -  x{l))2  -  1  0  0  0  -1  (3.39) 

where  „  indicates  that  the  position  of  the  ground  feature  used  is  unknown.  The 
nondimensional  measurement  error  is  8xf.  However,  in  order  to  achieve  full  aiding  [7] 
two  ground  features  are  always  used.  For  the  first  measurement  epoch  both  ground 
features  have  a  known  position,  for  the  second  epoch  there  is  one  ground  feature  position 
that  is  known  and  one  that  is  not,  and  for  the  remainder  of  the  measurement  epochs  both 
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ground  feature  positions  are  unknown.  The  transition  between  epochs  and  the  values  for 
x(l)  and  xp  will  be  discussed  more  in  Subsection  3.2.6. 

For  the  case  where  both  ground  points  are  known,  H  simplifies  to 

1  xPi  -  x(l)  0  0  2{x„\  -  x(l))  -  (xp\  -  x(l))2  0  0  0 

H  kkil)  =  (3.40) 

1  Xp2  -  x{l)  0  0  2(xp2  -  x(l))  -  ( Xp2  -  x{l))2  0  0  0 

where  *  indicates  that  the  position  of  the  ground  feature  used  is  known.  When  there  are 
two  subscripts,  as  will  always  be  the  case,  the  first  subscript  is  for  the  ground  feature 
closer  to  the  A/C  and  the  second  subscript  for  the  feature  further  away.  When  one  ground 
feature  position  is  known  and  the  other  is  not,  H  becomes 

1  xv\  -  x(l)  0  0  2 (xPi  -  x(l))  -  ( Xp\  -  x(l))2  0  0  0  0 

H  ku{l)  =  (3.41) 

1  Xp2  -  x(l)  0  0  l(xp2  -  x(l))  -  (xp2  -  x(i))2  0  0  0  -1 

Finally  for  the  case  where  both  ground  features  are  unknown,  H  is 

1  xp\-x(l )  0  0  2(xp]  -  x(l))  -  (Xp\  -  x(l))2  0  0  0  -1  0 

H  DU 

1  Xp2  -  x(l)  0  0  2(xp2  -  x(l))  -  (Xp2  -  x(l))2  0  0  0  0  - 1 

3.2.4  Initialization.  It  is  stipulated  that,  initially,  the  INS  has  zero  error  in  the 
navigation  states,  that  is,  the  alignment  was  perfect,  and  the  states  representing  the  biases 
in  the  sensors  are 

5f: <b)  -  N( 0,  o-2a)  Sf: <b)  -  N( 0,  cr2a)  6ca  -  N( 0,  cr2) 

The  x  and  z  accelerometers  are  of  the  same  quality.  Thus 

dx(0)  ~  N( 0,  P(0)) 

with  the  initial  covariance  matrix,  P(0)  given  by  Eq.  (3.22). 

3.2.5  Performance  of  Aided  INS.  The  cross  country  flight  scenario  is  shown  in 
Figure  3.2.  The  A/C  is  flying  wings-level,  at  a  constant  speed.  It  is  assumed  that  the 


(3.42) 
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Figure  3.2:  Initially  the  two  ground  features’  position  is  known,  but  in  the  second  epoch 
there  is  one  known  and  one  unknown  ground  feature,  where  the  unknown  feature’s  position 
was  estimated  by  the  A/C  at  the  end  of  the  first  epoch.  From  epoch  3  onward  both  ground 
features’  locations  are  not  exactly  known. 


ground  features  are  equally  spaced  one  kilometer  apart,  and  that  the  A/C  starts  one 
kilometer  behind  and  above  the  first  known  ground  feature.  In  the  observation  matrices 

Xf\  =  1  -  t 

Xf2  =  2  —  t 


3.2.6  Transitioning  Between  Measurement  Epochs.  For  the  purpose  of  covariance 
analysis,  and  because  the  features  were  equally  spaced,  and  it  took  exactly  one 
measurement  epoch  for  the  A/C  to  fly  from  directly  over  one  ground  feature  to  directly 
over  the  next  ground  feature,  the  relative  position  of  the  ground  features  to  A/C  was  the 
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same  at  the  beginning  of  each  measurement.  Therefore  the  universal  positions  of  the  A/C 
and  the  ground  features  are  not  considered.  After  non-dimensionalization  and 
discretization  the  position  of  the  A/C  relative  to  its  location  at  the  beginning  of  a 
measurement  epoch  is 

x(l)  =  /At,  0  <  /  <  N  -  1  (3.43) 


where  /  is  the  discrete  time  counter  within  a  measurement  epoch,  and  that  at  the  beginning 
of  each  epoch  /  starts  back  over  at  0.  There  are  360  epochs  to  be  considered.  Each  epoch’s 
duration  is  1  non-dimensional  second  and  in  each  epoch  N  =  100  bearing  measurements 
of  a  ground  feature  are  taken.  With  the  exception  of  the  first  and  last  ground  feature,  all 
the  ground  features  are  used  for  measurements  in  two  time  blocks/measurement  epochs. 
Throughout  the  entirety  of  the  calculations  xp\  =  1  and  xP2  =  2.  At  the  transition  point 
between  epochs,  the  old  xp2  becomes  the  new  xp]  and  a  new  xp2  is  acquired. 

Initially,  both  ground  features  used  for  measurements  were  known.  Therefore,  in  the 
first  epoch  the  measurement  matrix  Hkk(/),  and  the  dynamics  matrix  Aad8x8  were  used.  In 
the  first  epoch,  the  uncertainty  of  the  states  was  propagated  for  one  hundred  steps  using 
the  covariance  propagate  and  update  equations  of  the  Kalman  filter  [6] 


P(7+l)-=AadP(7)+Ad 


K  =  P(7+l)-Hkk(/)7 


n-l 


Hkk(/)P(/  +1)  Hkk(/)  +  R 


P(7  +  1)+  =  (I8  -  KHkk(/))P(/  +  1)~ 


(3.44) 

(3.45) 

(3.46) 


where  R  is  the  measurement  uncertainty  caused  by  ambiguity  in  the  camera  pixels 


and  the  measurement  noise 


R  = 


I  0 
0  l 


x  1(T6 


Sxf  i 
6xf2 


N( 0,  R) 


(3.47) 
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At  the  conclusion  of  the  first  one  hundred  steps,  the  first  ground  feature  was  dropped  from 
consideration,  and  a  new,  unknown  ground  feature  was  brought  in.  Thus  the  next  time 
block  required  the  use  of  the  new  dynamics  matrix  Aad9X9  from  Eq.  (3.20)  and  the 
measurement  matrix  Hku(/).  The  challenging  part  is  to  transition  the  covariance  matrix 
from  an8x8toa9x9  matrix,  while  including  the  correct  cross-covariance  terms.  This  is 
done  as  follows 


P(0)  = 


(3.48) 


J9x9 


P(99)8x8  P(l:8,l)(99) 

P(U:8)(99)  a-ew 

where  crfuw  is  the  uncertainty  in  position  of  the  new  ground  feature,  and  it  is  the 
uncertainty  in  the  A/C’s  position  at  the  time  the  new  ground  feature  is  acquired  plus  the 
uncertainty  of  the  measurement. 


°iew  -  P(i,o(99)  +  cr; 


(3.49) 


where  cr2£  was  the  uncertainty  caused  by  the  error  in  the  LOS  angle  measurement,  and  it 
had  a  non-dimensional  value  of  |  x  10-6.  It  is  because  of  the  correlation  of  the  errors  in 
the  navigation  state  x  and  the  new  state  xpi_  that  the  first  row  and  column  from  P8x8  must 
be  transplanted  to  their  respective  positions  in  P9X9.  Since  there  is  no  correlation  between 
the  LOS  error  of  the  camera  and  any  of  the  INS  navigation  or  bias  states,  it  is  not  added  to 
any  of  the  transplanted  fields.  The  covariance  matrix  is  then  propagated  in  the  same 
manner  as  in  the  first  epoch,  following  Eqs.  (3.44)-(3.46),  with  the  proper  substitution  of 
the  initial  covariance,  P(0)9X9  for  P(99)8x8,  dynamics,  Aad9x9  for  Aad8x8,  and  measurement 
matrices,  Hku  for  Hkk. 

The  transition  at  the  beginning  of  the  third  epoch  from  one  known/one  unknown  to 
two  unknown  ground  features  followed  the  same  pattern  as  incorporating  the  first 
unknown  ground  feature.  Now 


P(0)  = 


P(99)9x9  P(l:9,l)(99) 


P(l.l:9)(99) 


cr; 


10x10 


(3.50) 
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where 


cr 


2 

new 


P(U)(99)  +  m| 


(3.51) 


In  the  previous  epoch,  the  unknown  feature  was  xP2,  but  when  it  transitioned  to  xp\  status, 
all  of  its  cross-covariances  went  with  it.  Because  P( 9  9)  shows  the  uncertainty  of  the  closest 
feature  to  the  A/C,  xp\ ,  the  entirety  of  P(99)  could  be  directly  translated  to  the 
upper-diagonal  section  of  the  new  covariance  matrix.  Substituting  Huu  for  Hku,  and  using 
the  Aad  ioxio  dynamics  matrix,  the  covariance  was  propagated  according  to 
Eqs.  (3.44)-(3.46).  These  matrices  were  used  for  the  remainder  of  the  measurement 
epochs.  The  transition  becomes  more  complicated  when  the  A/C  completes  a  time  block 
using  two  unknown  ground  features,  and  begins  using  a  new  unknown  ground  feature. 

The  first  step  was  to  remove  the  ninth  row  and  column  of  P(99).  These  values  were 
replaced  with  the  first  eight  values  of  the  tenth  row  and  column  of  P(99),  as  shown 


trans  — 


(3.52) 


J9x9 


P(l:8,l:8)  P  (1:8.10) 

P(10,l:8)  P  (10.10) 

This  transition  matrix  was  then  used  to  initialize  the  new  state  estimation  error  covariance 
matrix,  Pi0xio 

r 

Ptrans  Ptrans(l:9,l) 

Ptrans(l,l:9)  ^  new 


P(0)  = 


(3.53) 


J 10x10 


where 


°\ew  =  Al,l)(99)  +  (r) 


(3.54) 


Starting  at  epoch  4,  the  transitions  for  the  remainder  of  the  epochs  followed 
Eqs.  (3.52)-(3.54)  from  epoch  3,  because  there  are  no  more  known  ground  features. 


3.3  Three-Dimensional  Horizontal  Case 

3.3.1  Dynamics.  The  navigation  frame  is  the  “inertial”  (Xn,  Yn,  Z„)  frame  and  the 
A/C  body  axes  are  ( Xb ,  Yb,Zh).  The  A/C’s  position  is  (x,y,  z),  and  ifr.  the  A/C  yaw  angle,  6, 
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Figure  3.3:  The  A/C  in  3-D  space.  Notice  that  the  origin  of  the  body  frame  is  located  at  the 
camera. 

the  A/C  pitch  angle,  and  <p  and  together  they  are  the  A/C’s  Euler  angles.  A  strapdown  [9] 
INS  arrangement  is  considered.  When  flying  over  a  non-rotating  and  flat  Earth  as  shown 
in  Figure  3.3,  the  dynamics  of  the  INS  errors,  also  known  as  the  error  equations,  are 
shown  in  state  space  notation  as  dx  =  A 6x  +  rdu,  where  the  navigation  state’s  position, 
velocity  and  angles  error 

dx  =  [  <5P  dV  dT  f 

and  the  disturbances  are  the  three  accelerometers’  and  the  three  rate  gyroscopes’  biases 

6u  =  t  8fXh  8fyb  8fZb  8ojx  6o)y  8a>z  \' 

Concerning  the  angular  errors  vector  c)T: 

dT  =  -dC^  (3.55) 
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and 


5 ¥  =  d'Fx  (3.56) 

where  <5T  is  the  skew  symmetric  matrix  formed  from  the  vector  d'P  according  to 
Eq.  (3.56). 

For  small  Euler  angles  ip,  6,  and  ep,  the  DCM 

1  -iff  6 

C"b(ip,  G,  ef>)  =  if,  1  -cp  (3.57) 

-G  ep  1 

and  therefore  its  perturbation 

0  -Sip  66 

SCI  =  Sip  0  -Sep  (3.58) 

-SO  Sep  0 

For  constant  altitude  flight  in  the  direction  of  the  Xn  axis,  the  nominal  Cbn  =  I3.  Thus, 
using  Eq.  (3.55)  we  calculate 

0  Sip  -66 

3^=  -Sip  0  Sep  (3.59) 

se  -Sep  0 

and  since  dT  =  civFx  we  recover  the  errors  in  the  A/C  Euler  angles 

w  =  [  -Sep  -56  -Sip  Y  (3.60) 

Hence,  the  navigation  state’s  error  vector  is 

Sx  =  [  Sx  Sy  Sz  Svx  Svy  Svz  -Sep  -SO  -Sip  Y  (3.61) 


The  INS  error  state  equations  are 


03x3 

1-3x3 

03x3 

03x3 

03x3 

Sx  = 

03x3 

03x3 

F(n) 
r  3x3 

Sx  + 

f^b 

03x3 

03x3 

03x3 

03x3 

03x3 

_ r^b 

(3.62) 
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where  F(,l)  =  /(,,)x  is  the  skew  symmetric  matrix  form  of  the  specific  force  vector.  The 
nominal  specific  force  components  during  constant  altitude,  wings  level  flight  are  fxn>  =  a , 
fyH]  =  0  and  f(7n)  =  -g,  where  g  is  the  acceleration  of  gravity  and  a  is  the  longitudinal 
acceleration  of  the  A/C.  Therefore 


a 

fn)  = 

= 

0 

_  fi"' . 

8 

(3.63) 


Eqs.  (3.62)  and  (3.63)  represent  the  dynamics  of  navigation  state’s  error,  ( SP ,  8  V,  <5VF), 
under  the  assumption  that  the  Earth  is  flat  and  non-rotating.  The  meaning  of  the  angular 
errors’  vector  dvF.  that  is,  its  relationship  to  the  Euler  angles’  errors,  has  been  determined 
by  the  A/C’s  trajectory,  that  is,  the  nominal  DCM  CJJ. 

Having  negative  angle  error  states  is  unorthodox.  In  order  for  the  navigation  state 
error  to  be 


fix  -  [  8x  8y  8z  8vx  8vy  8vz  8(f)  86  Sip  \' 
the  dynamics  Eq.  (3.62)  is  modified  as  follows 


03x3 

1-3x3 

03x3 

03x3 

03x3 

03x3 

03x3 

Tfi(n) 

r  3x3 

8x  + 

f'b 

03x3 

03x3 

03x3 

03x3 

03x3 

and  with  perfect  INS  alignment, 

<5x(0)  =  0 


(3.64) 


(3.65) 


Since  this  is  wings  level,  constant  altitude  flight,  in  the  direction  of  the  Xn  axis,  the 
nominal  navigation  variables  are 

1  2 

x  -  x0  +  vxt  +  -at  y  =  0  z  =  h 

0=0  0=0  0=0 
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These  variables  are  non-dimensionalized  as  follows 


the  non-dimensional  parameter  g  =  1 . 

It  is  assumed  that  the  sensor  errors  are  constant  biases  that  are  Gaussian  distributed. 
This  allows  the  state  error  vector  to  be  augmented  with  the  vector  <5u;  the  augmented  state 
is 

6x 

Sxa  =  ... 

du 

L  -*15xl 

and  the  dynamics  matrix  is  augmented  by  the  T  matrix,  as  shown 

a  r 

06x9  06x6 

J15xl5 
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One  obtains  a  dynamic  system  in  “free  fall”.  When  converted  to  discrete  time, 

Aa  — >  Aati  =  eAaAt,  where  At  is  the  sampling  interval.  The  augmented  discrete  time  state 
dynamics  become 

6xa(l  +  1)  =  Aaddxa(/),  /  =  0, . . . ,  N  -  1  (3.66) 


where  /  is  is  the  discrete  time  step  counter  during  a  measurement  epoch  and  the 
non-dimensional  time  step  is  AT  =  ^  :=  ATjr.  The  discrete-time  dynamics  matrix  can  be 
analytically  derived. 

This  dynamics  equation  applies  as  long  as  the  ground  features’  positions  are  known. 
Assuming  the  ground  features  are  stationary,  but  their  position  is  not  known,  two 
additional  states,  the  x  and  y  horizontal  coordinates  of  the  tracked  ground  features,  must 
be  added  for  each  tracked  ground  feature  whose  position  will  be  estimated  on  the  fly.  If 
the  number  of  ground  features  being  tracked  is  n,  then  the  augmented  navigation  state  is 

dxa 


dxa  :  = 


dxlA 


(3.67) 


8ypn 


(15+2n)xl 


and 


Aati  .— 


Aad  0i5x2« 

®2»xl5  ^2nx2n 


(15+2»)x(15+2n) 


(3.68) 


If,  for  example,  one  unknown  ground  feature  is  being  tracked  during  a  measurement 
epoch,  then  the  dimension  of  the  augmented  navigation  state’s  error  is  17  and  if  two 
unknown  ground  features  are  being  tracked  during  a  measurement  epoch  then  the 
dimension  of  the  navigation  state’s  error  is  19.  On  one  hand,  state  augmentation  reduces 
the  degree  of  observability,  which  decreases  the  strength  of  INS  aiding  action.  On  the 
other  hand,  the  inclusion  of  additional  features  to  be  tracked  increases  the  number  of 
measurement  equations,  which  helps  wash  out  the  measurement  error. 
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3.3.2  Modeling/Calibrating  the  Free  INS.  With  the  dynamics  from 
Subsection  3.3.1,  the  values  for  cra  and  cr„,  the  uncertainty  in  the  bias  of  the 
accelerometers  and  gyroscopes,  respectively,  are  set  such  that  the  free  INS  is  a  1^ 
navigation  system;  note  that  a  non-dimensional  hour  is  360  units  long.  Since  the  dynamics 
are  not  forced,  that  is,  there  is  no  controlled  input,  the  calibration  is  performed  by  using 
the  solution  to  the  Lyapunov  difference  equation,  Eq.  (3.21  with 


P(0)  = 


09x9  0  0 

0  diag  (cr2a,o-2a,o-2a)  0 

0  0  diag(cr;,  CTg,  a]) 


(3.69) 


The  Lyapunov  difference  equation  is  linear  and  therefore  there  is  a  linear  relationship 
between  the  uncertainty  in  the  sensors’  biases  and  the  ensuing  uncertainty  in  the  A/C’s  x 
position: 

PU(360AO  =  aa]  +  per]  (3.70) 


where  the  coefficients  a  and  / 3  are  constants.  Therefore,  Eq.  (3.21)  was  solved  for  one 
non-dimensional  hour  twice  to  calculate  the  values  of  the  constants  a  and  (3.  The  first 
time,  cra  was  set  to  1  and  crg  was  set  to  0.  The  second  time,  cra  was  set  to  0  and  crg  was  set 
to  1.  Then  assigning  the  errors  in  the  accelerometers  and  gyroscopes  an  equal  role/“guilt” 
in  the  uncertainty  of  the  A/C’s  position  at  time  360,  the  values  for  the  variances  of  the 
sensors’  biases  are  calculated  as 

c ra  =  — L  =  1.0912  x  10~5  (3.71) 

yjla 

ag  =  — U  =  9.0935  x  10~8  (3.72) 
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X 

Xp 

xf 

y 

= 

yP 

Rlos 

1 - b 

yf 

V x2f+y2f+f 2 

-f 

z 

Zp 

3.3.3  Measurement  Equation.  From  the  geometry  in  Figure  3.3  the  relationship  of 
the  inertial  position  of  the  A/C  to  that  of  the  ground  feature  P  is 


(3.73) 


where  xp,  yp,  and  zp  are  the  coordinates  of  the  ground  feature  in  the  inertial/navigation 
frame,  xj  and  y/  are  the  projections  of  the  ground  feature’s  respective  x  and  y  coordinates 
onto  the  focal  plane  of  the  camera  and  /  is  the  camera’s  focal  length.  For  the  case  when 
the  A/C  flies  wings  level  at  a  constant  altitude  in  the  direction  of  the  X„  axis  and  the  Euler 
angles  are  small,  the  DCM  for  relating  the  body  frame  to  the  navigation  frame  is  given  in 
Eq.  (3.57).  The  first  two  equations  in  the  relationship  given  by  Eq.  (3.73)  are  non-linearly 
dependent  on  the  third.  Now,  the  third  equation  yields 


and  thus 


Zn  ~Z 


I  Rlos  I 

l^j+yj  +  f2 


o  o  i  ]c/ 


I  Rlos  1 

.2  ,  ,,2  ,  n 


xf+yf  +  f 


Zn-Z 


0  0  1  C; 


'b  yf 


(3.74) 


Substituting  Eq.  (3.74)  into  Eq.  (3.73)  yields  the  two  measurement  equations  for  the  three 
dimensional  case: 


Zn-Z 


y  I  I  yP 


0  o  1  ]q  yf 

-f 


1  0  0 
0  1  0 


xf 

■m 

'b  y/ 
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Multiplying  out  the  matrices  yields 


X 

Xp 

1 

l 

'-s 

1 

_  y  _ 

_  yP . 

{p  - f-Oxf  +  tyf 

y/  +  */» A  +  f(p 

and  non-dimensionalizing  such  that 


xf 


f 


yf 


y_l 

f 


yields 


X 

Xp 

1 

xf  -  ifryf  -  6 

_  y  _ 

_  yP . 

p  -1  -  Oxf  +  (pyf 

yf  +  Xfip  +  (p 

We  obtained  two  separate  measurement  equations 


xp  -  x  - 


~{zp  -  z) 


xf  -  ipyf  -  6 
1  +  Oxf  -  (pyf 


yP  -y  =  -( Zp  -  z) 


yf  +  Xf\{j  +  (p 

1  +  Bxf  -  (pyf 


(3.75) 

(3.76) 


Due  to  the  small  angles  assumption,  the  denominator  in  Eqs.  (3.75)  and  (3.76)  can  be 
moved  up  such  that 


xp-x~  -(; zP  -  z)(xf  -  ifryf  -  60(1  -  0xf  +  <pyf)  (3.77) 

yP  -  y  ~  —(.Zp  -  z)(yf  +  xfip  +  <p)(  1  -  0xf  +  <pyf)  (3.78) 

Since  the  A/C  is  using  ground  features  to  aid  its  INS,  it  can  be  assumed,  without  loss  of 
generality,  that  zp  =  0.  Due  to  the  small  values  of  the  angles,  when  the  former  fraction  is 
distributed  out,  the  products  of  the  angles  are  negligible,  yielding 

Xp  -  x  =  z[xf  -  0(1  +  Xf)  +  (pxfyf  -  tpyf]  (3.79) 

yp  -y  =  z[yf  -  9xfyf  +  </>(!+  yj)  +  ipXf]  (3.80) 
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Next,  perturb  the  states  and  the  measurements 


x  -  xc-  8x 

e  =  ec-  86 


y  =  yc~8y  z  =  zc-8z 

(p  =  (pc  -  8(p  4/  =  ipc  -  81// 


Xp  =  Xpc  -  8xp  yp  =  ypc  -  8yp 

xf  =  (xfm-8xf)  yf  =  (yfm-8yf) 


where  the  subscript  c  indicates  calculated  values  provided  by  the  INS  and  the  subscript 
indicates  measured  quantities.  Inserting  the  perturbation  equations  into 
Eqs.  (3.79)  and  (3.80)  yields 


( x pr  xc )  +  8x  8xp  —  (z.c  8z)({xfm  8xj)  (6(:  80) ( 1  ■+■  [xjm  8xj)  )| 


+  (zc  ~  &)(  <Pc  -  8<p)(xfm-Sxf)(yfm-8yf)  -  (ijsc  -  8i//)(yfm-. 


Again,  due  to  the  small  error  in  the  measurements,  the  products  of  these  terms  can  be 
neglected. 


(xpc-xc)  +  8x  -  8xp  = 

(zc  ~  8z)((xfm-8xf)  -  ( 6C  -  86)(  1  +  xjm)  +  {(pc  -  8(p)xfmyfm  -  ( ipc  -  8ip)yfm\ 
Similarly,  in  the  second  measurement  equation 


( yPc-yc )  +  8y  -  8yp  =  (zc  -  8z){iyfm-8yf )  -  (6C  -  86)(xfm-8xf)(yfm 
+  (zc  -  8z)({(pc  -  8(p){  1  +  0 yfm-8yf)2)  +  (<pc  ~  Sip)(xfm-8xf) j 

=  fc  -  &)(  (y„,,-syf)  -  (ec  -  se)Xfmyfm  +  «,  -  sm  +  yjj  +  W.  -  ff^) 


Moving  all  the  error  terms  to  the  RHS  of  the  equation  and  all  the  non-error  terms  to  the 
LHS  yields 


{Xpc  Xc)  Zc{Xfm  6C{  1  +  X~j}n )  +  (pcXfmyfm  4* 8y fm)  ~  OX  8z{Xfm  6C(  1  +  Xj-fn ) 

+  < PcXfmyfm  'ArV/rn )  +  86{  1  +  Xj-m)Zc  8(p{XfmyfmZc)  +  fmZc)  "t"  8xp  SXfZc 

(3.81) 
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and 

(y pc  yc )  Zciy/m  QcXfmyfm  0c(  1  y fm)  tycXfm) 

-Sy-  Sziy/m  -  9cxfmyfm  +  0,(1  +  y2fm)  +  if/cxfm)  (3.82) 

+  5d{xfmyfmZc )  ~  50(1  +  ;y/m)zc  -  8if/(xfmzc )  +  5yp  -  5y/zc 

Finally,  non-dimensionalizing  such  that 

}'P  zP 

Xp  ^  h  yp  h  Zp  ^  h  ’ 

the  nondimensional  altitude  is  z  =  1 .  In  addition,  for  the  purpose  of  covariance  analysis, 
set  all  of  the  calculated  values  on  the  RHS  equal  to  the  nominal  values.  This  causes  all  of 
the  angles  to  go  to  zero,  simplifying  the  measurement  Eqs.  (3.81)  and  (3.82).  Also,  on  the 
RHS  set  Xfm  ■  =  Xf  andy/m  :=  V/. 


and 


i^Xpc  Xc)  Zc(xfm  6c(l  +  X~jnl )  +  (I)cXf 0 cy fm)  ~ 

-  8x  -  Szxf  +  50(1  +  x^)  -  8(j)Xfyf  +  Sifryf  +  8xp  -  6xy 


(3.83) 


(y  pc  yc)  Zc(yfm  9cXfmylm  +  0r(  1  +yym)  +  0c-^/m) 

-  Sy  -  Szy /  +  Sdxfyf  -  50(1  +  y2f)  -  SifjXf  +  Syp  -  Syf 


(3.84) 
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The  time  dependent  observation  matrix  H(7)  for  one  unknown  ground  feature  is 


-1  0 

0  -1 

-xf  -yf 

0  0 

0  0 

0  0 

-xfyf  -(1  +y2f) 

1  +  x)  xfyf 

H„(/)  =  yf  -Xf  (3.85) 

0  0 

0  0 

0  0 

0  0 

0  0 

0  0 

1  0 

0  1 

where  the  subscript  u  indicates  that  the  position  of  the  ground  feature  being  tracked  is 
unknown.  The  nondimensional  measurement  error  is  [ 8xf ,  6yj\ T . 

For  the  sake  of  observability  [7]  two  ground  features  will  be  tracked.  Therefore,  there 
will  be  two  subscripts.  The  first  will  correspond  to  the  ground  feature  that  is  closer  to  the 
A/C  and  the  second  to  the  ground  feature  that  is  further  away.  If  both  ground  features  are 
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known,  the  observation  matrix  is 


(3.86) 
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where  the  subscript  k  indicates  that  the  position  of  the  ground  feature  is  known.  When 


there  is  one  known  and  one  unknown  ground  feature,  the  observation  matrix  is 
H hud)  - 


-1 

0 

-1 

0 

0 

-l 

0 

-1 

-Xfl 

-yn 

-Xfl 

-yn 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-xfiyf  i 

-(i  +y}\) 

-Xfiyfi 

-( 1  +  >/2) 

1  +  4 

xnyn 

1  +x2f2 

Xfiyp- 

yf  i 

~Xfl 

y fi 

-Xfl 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

(3.87) 


40 


Finally,  when  neither  ground  feature’s  position  is  known,  the  observation  matrix 


H  „„(/)  = 


-1 

0 

-1 

0 

0 

-l 

0 

-1 

~Xf\ 

-y/ 1 

-Xf2 

-yf2 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

-Xfiyn 

-(i  +y2n) 

-Xfiy/2 

-('  +>7 

1  + 

x/iyf  i 

l+x2f2 

Xfiyfi 

yi  i 

~Xf\ 

y/2 

-Xfl 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

i 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

(3.88) 


3.3.4  Performance  of  Aided  INS.  The  cross  country  flight  scenario  is  shown  in 
Figure  3.2.  The  A/C  is  flying  wings-level,  at  a  constant  speed.  It  is  assumed  that  the 
ground  features  are  equally  spaced  one  kilometer  apart,  and  that  the  A/C  starts  one 
kilometer  behind  and  above  the  first  known  ground  feature.  It  is  also  assumed  that  the 
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ground  features  lay  on  the  A/C’s  ground  track.  In  the  observation  matrices 


Xf\(t)  =  1  -  t 


Xfiif)  =  2  -t 


=  yfi(t)  =  0 


3.3.5  Initialization.  It  is  stipulated  that,  initially,  the  INS  has  zero  error  in  the 
navigation  states,  that  is,  the  INS  alignment  was  perfect,  and  the  states  representing  the 
biases  in  the  sensors  are 


■vf 

Sio, 


N{ 0,  (r2a)  6fyb)  ~  N( 0,  or2a)  Sf®  ~  N( 0,  or2a) 

N( 0,  crj)  Sojy  ~  N( 0,  (T2)  6(oz  ~  Nil),  a]) 


The  x,  y  and  z  accelerometers  are  of  the  same  quality;  also  the  x,  y  and  z  gyroscopes  are  of 
the  same  quality.  Thus 

dx(0)  ~  N( 0,  P(0)) 

with  the  initial  covariance  matrix  P(0)  given  by  Eq.  (3.69). 


3.3.6  Transitioning  Between  Measurement  Epochs.  For  the  purpose  of  covariance 
analysis,  and  because  the  features  were  equally  spaced  and  it  took  exactly  one  time  block 
for  the  A/C  to  fly  from  directly  over  one  ground  feature  to  directly  over  the  next  ground 
feature,  the  relative  position  of  the  ground  features  to  the  A/C  was  the  same  at  the 
beginning  of  each  time  block/measurement  epoch.  Therefore  the  absolute  positions  of  the 
A/C  and  the  ground  features  are  not  considered.  After  non-dimensionalization,  the 
position  of  the  A/C  relative  to  its  location  at  the  beginning  of  a  measurement  epoch  is 

x(l)  =  lAt,  y(l)  =  0,  0  <  /  <  N  -  1  (3.89) 

Recall  that  /  is  the  discrete  time  counter  within  a  measurement  epoch,  and  that  at  the 
beginning  of  each  epoch  /  starts  back  over  at  0.  There  are  360  epochs  to  be  considered. 
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Each  epoch’s  duration,  T,  is  1  non-dimensional  second  and  in  each  epoch  N  =  100 
bearing  measurements  of  a  ground  feature  are  taken.  With  the  exception  of  the  first  and 
last  ground  feature,  all  the  ground  features  are  used  for  measurements  in  two  consecutive 
time  blocks/measurement  epochs.  Throughout  the  entirety  of  the  calculations  xpl  =  1  and 
xP2  =  2.  The  lateral  positions  of  the  ground  features  are  0.  At  the  transition  point  between 
epochs,  the  old  xp2  and  yp2  becomes  the  new  xp]  and  yp] ,  respectively,  and  a  new  ground 
feature  is  acquired. 

Initially,  the  positions  of  both  tracked  ground  features  were  assumed  known. 
Therefore,  in  epoch  1  the  observation  matrix  Hkk(/),  and  the  dynamics  matrix  Aadl5xl5 
were  used.  In  the  first  epoch,  the  uncertainty  of  the  states  was  propagated  for  one  hundred 
steps  using  the  covariance  propagate  and  update  equations,  Eqs.(3.44)-(3.46),  substituting 
in  the  correct  values  for  the  dynamics,  observation  and  identity  matrices,  where  R  is  the 
measurement  uncertainty  caused  by  one  pixel  in  the  camera’s  focal  plane 


N(  0,R) 


Sxfl 

6yfl 
6xf2 
6yf2 

We  assume  a  9  Megapixel  camera  with  an  aspect  ratio  of  1  and  therefore  the 
nondimensional 


I  0  0  0 
0  \  0  0 

R  =  9  x  1(T6 

0  0  |  0 

0  0  0  i 

At  the  conclusion  of  the  first  one  hundred  steps/the  first  measurement  epoch,  the  first 
ground  feature  was  dropped  from  consideration,  and  a  new,  unknown  ground  feature  was 
brought  in.  Thus  the  next  time  block  required  the  use  of  the  augmented  dynamics  matrix 
Aadl7xl7  from  Eq.  (3.68)  and  the  observation  matrix  Hku(/). 
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The  challenging  part  is  to  transition  the  covariance  matrix  P  from  an  15  x  15  to  a 
17  x  17  matrix,  while  including  the  correct  cross-covariance  terms.  This  is  done  as 
follows:  when  transitioning  from  two  known  ground  features  to  one  known/one  unknown 
ground  feature,  the  new  initial  covariance  matrix  is 


P(0) 


P(99) 


15x15 


P(99)(i:i5,i)  P(99)(i:i5  2) 


P(99)(U:i5) 

P(99)(2;1:15} 


cr- 


P(  99) 


1,2 


P(99)1;2 


cr: 


ynew 


(3.90) 


J  17x17 


where  crxnew  is  the  uncertainty  in  the  x  position  of  the  new  ground  feature,  and  it  is  the 
uncertainty  in  the  A/C’s  x  position  at  the  time  the  new  ground  feature  is  acquired  plus  the 
uncertainty  brought  about  by  the  optical  measurement: 


cT2xnew  =  P(lA)m  +  ^  (3.91) 

Similarly,  cr^new  is  the  uncertainty  in  the  y  position  of  the  new  ground  feature,  and  it  is  the 
uncertainty  in  the  A/C’s  y  position  at  the  time  the  new  ground  feature  is  acquired  plus  the 
uncertainty  brought  about  by  the  optical  measurement: 


=  W  99)  +  o$  (3.92) 

where  cr^  and  cr^  are  the  uncertainties  caused  by  the  error  in  the  LOS  angle  measurements, 
and  they  each  have  a  non-dimensional  value  of  |  x  10-6.  It  is  because  of  the  correlation  of 
the  errors  in  the  A/C  navigation  state  x  and  the  new  ground  feature’s  position  state  jcp2  that 
the  first  row  and  column  from  Pi5Xis  must  be  transplanted  to  their  respective  positions  in 
P [7x17-  Since  there  is  no  correlation  between  the  LOS  error  of  the  camera  and  any  of  the 
INS  navigation  or  bias  states,  it  is  not  added  to  any  of  the  transplanted  fields.  The  same 
holds  true  for  the  navigation  state  y  and  the  new  state  y/)2.  The  covariance  matrix  is  then 
propagated  in  the  same  manner  as  in  the  first  epoch,  following  Eqs.  (3.44)-(3.46),  with  the 
proper  substitution  of  the  initial  covariance,  P(0)nxi7  for  P(99)i5xi5,  dynamics,  Aadl7xl7 
(found  using  Eq.  3.68)  for  Aadl5xl5,  and  observation  matrices,  Hku  for  Hkk. 
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The  transition  at  the  beginning  of  the  third  epoch  from  one  known/one  unknown  to 
two  unknown  ground  features  follows  the  same  pattern  as  incorporating  the  first  unknown 
ground  feature.  Now 


P(0)  = 


P(99)17x17  P(99)(1:17j1)  P(99)(1:17>2) 
P(99)(U:i7)  °~xnew  P(99)i>2 

P(99)(2,1:17)  P(  99)u 


(3.93) 


where  cr2xnew  and  cr2new  are  calculated  according  to  Eqs.  (3.91)  and  (3.92). 

In  the  previous  epoch,  the  unknown  feature’s  position  was  (xP2,  ypi),  but  when  it 
transitioned  to  being  the  closer  ground  feature,  all  of  its  cross-covariances  went  with  it. 
Because  Pa6,m  and  Pi7,i7  show  the  uncertainty  of  the  closest  feature  to  the  A/C,  the 
entirety  of  P(99)  could  be  directly  translated  to  the  upper-diagonal  section  of  the  new 
covariance  matrix.  Substituting  Huu  for  Hku,  and  using  the  Aadl9xl9  dynamics  matrix,  the 
covariance  was  propagated  according  to  Eqs.  (3.44)-(3.46).  These  matrices  were  used  for 
the  remainder  of  the  measurement  epochs. 

The  transition  becomes  more  complicated  when  the  A/C  completes  a  time  block 
using  two  unknown  ground  features,  and  begins  using  a  new  unknown  ground  feature.  A 
transition  matrix  is  required 


P  = 


P(99)i5xl5  P(99)(1;15,18)  P(99)(1:16,19) 

P(99)(18,1:15)  P(99)18,18  : 

P(99)(19;1:16)  .  .  .  P(99)i9,i9 


(3.94) 


This  transition  matrix  was  then  used  to  initialize  the  new  state  estimation  error  covariance 
matrix,  Pi9xi9: 


P(0)  = 


P 


P  (1,1:17) 
P(2,l:17) 


E*  (1:17,1) 

P(1 : 17,2) 

CT2 

xnew 

P(l,2) 

P(  1,2) 

CT2 

ynew 

19x19 


(3.95) 
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where  cr2xnew  and  cr^new  are  calculated  according  to  Eqs.  (3.91)  and  (3.92).  Starting  at  epoch 
4,  the  transitions  for  the  remainder  of  the  epochs  followed  Eqs.  (3.94)-(3.95)  from  epoch 
3,  because  there  are  no  more  known  ground  features. 

3.4  Three  Dimensional  Vertical  Case 

The  inertial  and  body  frames  are  same  as  for  horizontal  case  laid  out  in 
Subsection  3.3.1.  When  flying  towards  a  flat,  non-rotating  Earth  the  reference  frame 
relationship  is  shown  in  Figure  3.4.  For  this  case  the  body  axes  have  been  rotated  away 
from  the  body  fixed  frame  in  order  for  them  to  remain  aligned  with  the  navigation  axes. 
This  was  done  to  maintain  the  established  linear  mathematics  model.  To  keep  the  body 
axes  aligned  such  that  the  Xb  axis  is  the  longitudinal  axis,  one  must  use  the  nonlinear 
dynamics  model  found  in  Appendix  D  and  the  nonlinear  measurement  model  found  in 
Section  E.2.  For  this  case  it  is  assumed  that  v(0)  =  0. 

3.4.1  Dynamics.  The  dynamics  model  is  the  same  as  the  horizontal  case,  shown  in 
Eq.  (3.65).  The  primary  difference  is  that  the  nominal  specific  force  components  during  a 
perfect  vertical  drop  will  be  time  varying.  Until  the  munition  reaches  terminal  velocity  all 
of  the  nominal  specific  forces  are  zero.  Therefore 


/f 

0 

II 

1? 

f 

= 

0 

V  0  <  t  <  tferm 

(3.96) 

tin) 

0 

Once  the  munition  reaches  terminal  velocity,  at  tterm,  the  accelerometers  will  detect 
nominal  specific  forces  such  that  f(xn)  =  ax,  fy  =  ay,  and  f7n>  =  g  where  g  is  the 
acceleration  of  gravity  and  ax  and  ay  are  the  acceleration  components  of  the  munition 
along  its  Xb  and  Yh  axes.  Since  a  purely  vertical  drop  is  considered,  ax  =  ay  =  0. 
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Figure  3.4:  The  munition  falling  in  3-D  space.  Notice  that  the  origin  of  the  body  frame  is 
located  at  the  camera. 


Therefore, 


fxt] 

0 

S' 

II 

fln) 

= 

0 

V  tterm  <  t  <  T 

(3.97) 

fzn) 

8 

Eqs.  (3.65,  3.96,  3.97)  represent  the  time- varying  dynamics  of  navigation  state’s  error. 

Since  this  is  a  vertical  drop  and  the  nominal  trajectory  is  such  that  the  body  axes  are 
aligned  with  the  navigation  axes,  the  time  history  of  the  nominal  navigation  variables  is 


x  =  0,  y  =  0,  and 


z(t )  = 


hre i  -  !  t2 


V  0  <t  <tu 


lrel  2  * 

hterm  ^ 'termfterm  ^ termf  ^  t term  —  t  —  T 
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(/>  =  6  =  iJj  =  0,  where  htenn  and  vterm  are  the  altitude  and  velocity  of  the  munition  when  the 
munition  reaches  terminal  velocity.  These  variables  are  non-dimensionalized  as  follows 


v 

th’ 

h 

•  Lfprt 


T- 

h 

r  term 


where  h  and  v  are  a  typical  altitude  and  velocity  when  flying  wings-level,  t  is  the 
current  time,  and  T  =  7  is  the  nondimensional  fall  duration. 

The  remainder  of  the  dynamics  model  is  identical  to  that  of  Subsection  3.3.3. 


3.4.2  Modeling/Calibrating  the  Free  INS.  With  the  dynamics  from 
Subsection  3.3.1,  the  values  for  cra  and  crg,  the  uncertainty  in  the  bias  of  the 
accelerometers  and  gyroscopes,  respectively,  are  set  such  that  during  wings  level 
horizontal  flight  the  free  INS  is  a  100^  navigation  system;  note  that  a  non-dimensional 
hour  is  360  units  long.  Since  the  dynamics  are  not  forced,  that  is,  there  is  no  controlled 
input,  the  calibration  is  performed  by  using  the  solution  to  the  Lyapunov  difference 
equation,  Eq.  (3.21)  with  P(0)  from  Eq.  (3.69) 

The  Lyapunov  difference  equation  is  linear  and  therefore  there  is  a  linear  relationship 
between  the  uncertainty  in  the  sensors’  biases  and  the  ensuing  uncertainty  in  the 
munition’s  x  position: 

Pi,i(360A0  =  acr2a  +  f3crg 

where  the  coefficients  a  and / 3  are  constants.  Therefore,  Eq.  (3.21)  was  solved  for  one 
non-dimensional  hour  twice  to  calculate  the  values  of  the  constants  a  and  f3.  The  first 
time,  cra  was  set  to  1  and  crg  was  set  to  0.  The  second  time,  cra  was  set  to  0  and  crg  was  set 
to  1.  Then  assigning  the  errors  in  the  accelerometers  and  gyroscopes  an  equal  role/“guilt” 
in  the  uncertainty  of  the  munition’s  position  at  time  360,  the  values  for  the  variances  of  the 
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sensors’  biases  are  calculated  as 


crfl  =  =  1.0912  x  10~3  (3.98) 

V2^ 

cr g  =  ——  =  9.0935  x  10~6  (3.99) 

3.4.3  Measurement  Model.  Because  the  body  reference  frame  is  aligned  with  the 
navigation,  the  measurement  model  developed  in  Eqs.  (3.73)-(3.82)  holds  true. 
Nondimensionalizing  such  that 

xp  yp  Zp 

Xp  J  y,,^J  Zp  ^  T’ 


the  nondimensional  altitude  is  zc  =  Zj.  In  addition,  for  the  purpose  of  covariance  analysis, 
set  all  of  the  calculated  values  on  the  RHS  equal  to  the  nominal  values.  This  causes  all  of 
the  angles  to  go  to  zero,  simplifying  the  measurement  Eqs.  (3.81)  and  (3.82).  Also,  on  the 
RHS  set  Xfm  '■=  Xf  and  y/m  :=  y/. 


and 


( X  p(:  Xr)  Zc(xfm  0C(  1  +  Xjm )  +  <pcXfmy  f,n  lf/cyfm)  — 

-  Sx  -  dzxf  +  S0(\  +  x2f)zi  -  5<pXfyfzi  +  Si//yfzi  +  Sxp  -  6xfzi 


(3.100) 


(y pc  yd)  Zciy/m  QcXfmyfm  4>r{  1  ~\~  y  fm )  +  ijj cXf!u )  — 

-  <5y  -  dzyf  +  56xfyfzi  -  6<p(  1  +  y2f)z/  -  6if/xfzi  +  Syp  -  8yfzi 


(3.101) 
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The  time  dependent  observation  matrix  H(7)  for  one  unknown  ground  feature  is 


H  „(/)  = 


0 

~xf 

0 

0 

0 

-xfyfzi 
(1  +Xf)Zi 

yw 

0 

0 

0 

0 

0 

0 

1 

0 


0 

-1 

-yf 

0 

0 

0 

-(1  +y2f)zi 
xfyfzi 

-XfZl 

0 

0 

0 

0 

0 

0 

0 

1 


(3.102) 


where  the  subscript  u  indicates  that  the  position  of  the  ground  object  being  tracked  is 
unknown.  The  nondimensional  measurement  error  is  [ 6xf ,  6yj\ T. 


50 


For  the  sake  of  observability  [7]  two  ground  objects  will  be  tracked.  The  observation 


matrix  for  tracking  two  unknown  ground  features 
H  uu(l)  = 

r  P 


-1 

0 

-1 

0 

0 

-1 

0 

-1 

-.*/! 

-y/ 1 

-Xf2 

-yp- 

0 
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0 

0 

0 

0 

0 

0 

-Xfiy/izi 

-0+y2f])zi 

-x/iyfiz, 

-(i  +y2f2P 

(1  +x2fl)zi 

Xfiy/izi 

(1  +x2f2)zi 

xp.ypzi 
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~xflZi 
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~XpZi 

0 

0 

0 

0 

0 
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0 

0 

0 

0 

0 
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0 
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0 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 

3.4.4  Performance  of  Aided  INS.  The  munition  is  in  a  free  fall,  that  is,  vz(0)  =  0 
and  its  acceleration  is  —g  along  the  Z„  axis  until  it  reaches  terminal  velocity.  The  nominal 
vertical  drop  is  such  that  hrei  =  7500  [/»].  The  terminal  vterm  =  100  “  so  that  the  free  fall  is 
10  seconds  and  the  duration  of  the  vertical  drop  is  80  seconds.  The  nominal  trajectory  is 
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Figure  3.5:  The  locations  of  the  tracked  ground  features  in  the  first  geometry. 


x(t)  =  0,  y(t)  =  0  and 


z(t) 


7500  -  5 12  V  0  <  t  <  tlerm 
7000  -  100(7  -  ttenn)  V  tterm  <  t  <  T 


Once  the  terminal  velocity  has  been  reached  there  is  no  further  acceleration  and  the 
vertical  speed  is  constant.  Two  ground  feature  geometries  were  considered.  The  first  is 
shown  in  Figure  3.5.  For  this  geometry,  in  the  observation  matrix 


Xfl(l)  = 


xpi 

Zi 


XflSJ) 


Xp2 

ZI 


.025 


Zi 

-.025 


Zi 


yf\0)  =  yf2(i)  =  o,  i  =  o,...,n-i 
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Normalized  Ground  Feature  Locations  Geometry  2 
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Figure  3.6:  The  locations  of  the  tracked  ground  features  in  the  second  geometry. 


The  second  geometry  is  shown  in  Figure  3.6.  For  this  geometry,  in  the  observation 
matrix 


Xf](l) 


xPi 

Zi 


.025 

Zi 


Xfi{l)  =  V/i  (/)  =  o 


yfi(l)  = 


ypi 

Zi 


.025 

9 

Zi 


I  =  0, ...  ,N  —  l 


3.4.5  Initialization.  It  is  stipulated  that  initially,  the  INS  has  zero  error  in  the 
navigation  states,  that  is,  the  INS  alignment  was  perfect,  and  the  states  representing  the 
biases  in  the  sensors  are 


d/f 

Sojx 


NiO,  oj)  Sf. 'ib)  ~  N( 0,  or2a)  8f: f  ~  N( 0,  or2a) 

N( 0,  crj)  6coy  ~  N( 0,  a])  8ojz  ~  N(0,  a]) 


The  x,  y  and  z  accelerometers  are  of  the  same  quality,  and  also  the  x,  y  and  z  gyroscopes 
are  of  the  same  quality.  Thus 

dx(0)  ~  N( 0,  P(0)) 
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with  the  initial  covariance  matrix  P(0)  given  by  Eq.  (3.69). 


3.4.6  Transitioning  Between  Measurement  Epochs.  In  the  pure  free  fall  case,  there 
are  three  measurement  epochs.  The  first  is  while  the  munition  is  accelerating  toward  the 
earth.  The  duration  of  the  first  epoch  is  one  nondimensional  second.  There  is  N  =  100 
discrete  steps  per  nondimensional  second. 

Therefore,  in  epoch  1  the  observation  matrix  Huu(/),  and  the  dynamics  matrix 
Aadisxis  based  on  the  nominal  accelerometer  values  from  Eq.  (3.96)  were  used.  In  the  first 
epoch,  the  uncertainty  of  the  states  were  propagated  for  one  hundred  steps  using  the 
covariance  propagate  and  update  equations  of  the  Kalman  filter,  Eqs.  (3.44)(3.46),  where 
R  is  the  measurement  uncertainty  caused  by  one  pixel  in  the  camera’s  focal  plane 


5xf  i 
6yfl 
6xf2 
5yP_ 


N(  0,R) 


We  assume  a  9  Megapixel  camera  with  an  aspect  ratio  of  1.  However,  recall  from 
Eqs.  (3.100)  and  (3.101)  that  the  measurement  error  terms  on  the  RHS  are  multiplied  by 
the  time-varying  altitude  zi .  Therefore,  the  nondimensional 


R  =  z? 


I  0  0  0 


0  i  0  0 


x  10 


(3.104) 


0  0  |  0 

0  0  0  ± 

where  zi  is  squared  to  match  R.  If  R  was  standard  deviation  of  the  noise  instead  of  the 
variance  then  z\  would  not  need  to  be  squared. 

At  the  conclusion  of  the  first  one  hundred  steps/the  first  measurement  epoch  the 
munition  reached  terminal  velocity.  Thus  the  next  time  block  required  the  use  of  the 
dynamics  matrix  Aadl5xl5  to  be  based  on  the  nominal  accelerometer  values  from 
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Eq.  (3.97).  The  second  measurement  epoch  was  6  nondimensional  seconds  long.  Without 
any  further  transitions,  Eqs.  (3.44)-(3.46)  were  repeated  for  101  <  l  <  700,  using  the 
updated  dynamics  matrix. 

We  assumed  a  camera  Field  of  View  (FOV)  of  50  milliradians.  Based  on  the 
geometry  set  forth  in  Figure  3.5,  the  ground  features  leave  the  camera  FOV  when  the 
munition  is  1000  meters  above  the  ground,  hfinai.  Thus  the  third  and  final  measurement 
epoch  started  when  the  munition  reached  hf[na\  in  70  seconds.  In  the  final  10  seconds  of  its 
flight  the  INS  is  not  aided,  and  one  reverts  to  a  free  INS.  As  such  the  calculations  using 
the  KF  equations,  (3.44)-(3.46),  were  terminated  and  the  calculation  for  the  free  INS, 

Eq.  (3.21),  was  used  for  the  last  10  seconds. 

This  process  was  completed  for  both  geometries  listed  at  the  beginning  of 
Subsection  3.4.4. 
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4  Covariance  Analysis  Results 


4.1  Introduction 

This  chapter  is  broken  up  into  the  three  sections:  Section  4.2  covers  the  two 
dimensional  case,  Section  4.3  shows  the  case  where  the  A/C  was  flying  horizontally,  and 
Section  4.4  discusses  the  case  where  a  munition  is  in  vertical  free  fall,  for  both  of  the 
geometries  considered.  All  three  sections  will  start  with  baseline  plots  showing  the 
development  of  the  standard  deviations  of  the  position  estimates  from  the  unaided  INS. 
This  information  will  then  be  followed  by  plots  of  the  aided  INS.  Plots  showing  the 
remainder  of  the  navigation  states,  for  both  the  unaided  and  aided  INS  schemes  are  found 
in  Appendices  A-C. 

4.2  Two  Dimensional  Results 

Initially  the  standard  deviation  of  the  navigation  states  of  the  unaided  INS  were 
plotted  as  a  baseline.  As  expected,  the  standard  deviation  of  the  x  position  was  1  kilometer 
after  one  hour.  The  standard  deviations  for  the  x  and  z  positions  are  shown  in  Figure  4.1. 
There  was  a  significant  degree  of  aiding  achieved  with  this  scheme,  shown  in  Figure  4.2. 
After  an  hour  of  flight  the  standard  deviation  in  the  x  position  is  only  five  meters,  an 
improvement  of  99.5%  off  of  the  unaided  system.  If  one  multiplies  the  position  standard 
deviations  produced  by  the  unaided  INS  scheme,  the  area  of  uncertainty  is  calculated 

o-area  =  crx(360N)  x  crz(360N)  =  .707 1  [km2]  (4.1) 

That  means  that  the  A/C  is  likely  to  be  somewhere  inside  a  square  with  that  area.  That 
being  said,  if  the  same  process  is  applied  to  the  aided  scheme 

c rarea  =  cr t(3 60 A)  x  cr,(360A)  =  1.8771  x  \(r7[km2\  (4.2) 

There  is  an  eight  order  of  magnitude  improvement  in  the  uncertainty  of  the  position 
estimate  of  the  aided  case  over  the  unaided  case! 
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But  the  x  position  was  not  the  only  navigation  state  that  was  aided.  The  other  four 


states  showed  an  order  of  magnitude  reduction  in  uncertainty  as  well,  shown  in 
Figures  A.1-A.4.  A  side-by-side  comparison  of  the  navigation  state  vector  standard 
deviations  is  shown  in  Table  4.1. 

In  the  best  tradition  of  SLAM,  the  aiding  action  factored  into  the  uncertainty  of  the 
ground  objects’  position  as  well.  Notice  that  in  Figure  A. 5  the  ground  objects’  position 
standard  deviation  trends  similarly  as  the  x  position  standard  deviation.  A  closer 
inspection  of  the  ground  objects’  position  uncertainty  in  Figure  A. 6  shows  how  at  the 
beginning  of  each  epoch  there  is  a  spike  for  xp2.  This  is  due  to  the  initialization  error  from 
the  LOS  angle  error  of  the  camera.  There  is  a  jump  in  the  position  uncertainty  of  xp\  as 
well,  but  it  is  far  less  drastic  than  for  xp2,  but  that  is  because  the  position  uncertainty  of  xp2 
at  the  end  of  an  epoch  becomes  the  position  uncertainty  of  xp\  at  the  beginning  of  the  next 
epoch. 


Table  4.1:  The  peak/final  standard  deviation  values  for 
the  unaided  navigation  states.  Also  included  are  the  peak 
and  final  standard  deviations  for  aided  navigation  states 
for  the  two-dimensional  case. 


Standard  Deviation 

Unaided  Final  Value 

Aided  Peak  Value 

Aided  Final  Value 

crx 

1  [km ] 

4.7  [m] 

4.7  [m] 

crz 

.7071  [km] 

.1  [m] 

.04  [m] 

&  Vx 

7  X  1 0  3  [m/s] 

8.21  x  10~5  [m/s] 

4.0  x  10~5  [m/s] 

Vz 

4  x  10“3  [m/ s] 

3.9  x  10  5  [m/s] 

2.2  x  10-7  [m/s] 

(T9 

3.27  x  10~5  [rad] 

3.31  x  10~6  [rad] 

2.44  x  10~7  [rad] 
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2-d  Unaided  Position  Uncertainty 


Figure  4.1:  The  2-d  standard  deviation  of  the  position  states  in  the  unaided,  free  INS 

4.3  Horizontal  Three  Dimensions 

The  results  for  the  three  dimensional  calculations  showed  strong  correlation  to  the 
two  dimensional  case,  though  more  insight  was  offered  to  the  attitude  navigation  states. 

As  expected  the  unaided  INS  had  a  standard  deviation  for  the  x  and  y  positions  of  1 
kilometer  each,  shown  in  Figure  4.3  with  the  improvement  produced  by  the  aiding  scheme 
shown  in  Figure  4.4.  Once  again  the  horizontal  position  uncertainties  dropped  to  a  little 
below  five  meters.  If  we  multiply  the  position  standard  deviations  produced  by  the 
unaided  INS  scheme,  we  calculate  a  volume  of  uncertainty 

o-voi  =  crx(360N)  x  crv(360A)  x  cr-(360A)  =  .707 1  [km3]  (4.3) 

That  means  that  the  A/C  is  very  likely  to  be  somewhere  inside  a  box  with  that  volume. 
That  being  said,  if  the  same  process  is  applied  to  the  aided  scheme 

crvo/  =  crr(36OA0  x  crv(36OA0  x  cr,(360A)  =  7.4177  x  10“10[km3]  (4.4) 
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Figure  4.2:  The  2-d  standard  deviation  of  the  position  states  in  the  aided  INS 

There  is  an  eleven  order  of  magnitude  improvement  in  the  uncertainty  of  the  position 
estimate  of  the  aided  case  over  the  unaided  case!  Five  of  the  other  six  navigation  states 
showed  an  order  of  magnitude  improvement  as  well,  seen  by  comparing  the  peak  and  final 
navigation  states’  standard  deviations  shown  in  Table  4.2.  The  temporal  development  of 
the  standard  deviations  of  the  velocity  and  attitude  states  is  shown  in  Figures  B.1-B.4. 

Once  again,  there  is  a  relationship  between  the  uncertainty  of  the  A/C  position 
estimate  and  the  ground  features’  position  uncertainty.  As  shown  in  Figures  B.5  and  B.6 
the  x  and  y  position  standard  deviations  of  the  ground  features  trend  similarly  to  the  x  and 
y  position  standard  deviations  of  the  A/C. 
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3-d  Unaided  Position  Uncertainty 


Figure  4.3:  The  3-d  standard  deviation  of  the  position  states  in  the  unaided,  free  INS 

4.4  Vertical  Three  Dimensions 

Once  again  there  was  a  considerable  decrease  in  the  uncertainty  of  the  position 
estimate  from  the  unaided  case,  Figure  4.5,  to  the  two  aided  schemes,  Figures  4.6  and  4.7. 
The  final  standard  deviations  of  the  unaided  x,  y,  and  z  positions  are  about  35  meters.  For 
the  first  geometry,  the  horizontal  position  estimates  had  standard  deviations  of  about  12 
centimeters,  with  the  z  standard  deviation  ending  around  4.3333  meters.  The  second 
geometry  produced  x,  y  and  z  standard  deviations  of  12  centimeters,  17  centimeters  and  1 
micrometer  respectively.  This  shows  that  by  removing  the  symmetry  in  the  ground  feature 
geometry  the  uncertainty  of  the  altitude  state  drops  greatly.  To  put  this  in  another 
perspective,  the  unaided  volume  of  uncertainty  is  4.1  x  104  m3  and  the  aided  volume  of 
uncertainty  for  the  second  geometry  is  2.05  x  10-8  nr\ 


60 


3-d  Aided  Position  Uncertainty 
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Figure  4.4:  The  3-d  standard  deviation  of  the  position  states  in  the  aided  INS 


The  temporal  development  of  the  unaided  standard  deviation  for  the  velocity  and 
attitude  states  is  shown  in  Figures  C.1-C.2.  The  temporal  development  of  the  aided 
standard  deviations  for  both  geometries  is  shown  in  Figures  C.3-C.10.  The  peak  and  final 
standard  deviations  for  the  first  geometry  are  listed  in  Table  4.3 
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Table  4.2:  The  peak/final  standard  deviation  values  for 
the  unaided  navigation  states.  Also  included  are  the  peak 
and  final  standard  deviations  for  aided  navigation  states 
for  the  three-dimensional  horizontal  case. 


Standard  Deviation 

Unaided  Final  Value 

Aided  Peak  Value 

Aided  Final  Value 

crx 

1  [km] 

4.7  [m] 

4.7  [m] 

CTy 

1  [km] 

4.0  [m] 

4.0  [m] 

<rz 

.7071  [km] 

0.10  [m] 

0.0389  [m] 

0"  Vx 

7  x  10~3  [m/s] 

8.21  x  10~5  [m/s] 

3.93  x  10-5  [m/s] 

CT  Vy 

7  X  10-3  [m/s] 

3.06  x  10~5  [m/s] 

2.29  x  10~5  [m/s] 

0"  Vz 

4  x  10-3  [m/  s] 

3.9  x  10  5  [m/s] 

2.16  x  10~7  [m/s] 

0"  0 

3.27  x  10~5  [rad] 

2.40  x  10~6  [rad] 

1.07  x  10-7  [rad] 

0"  e 

3.27  x  10~5  [rad] 

3.16  x  10~6  [rad] 

2.39  x  10~7  [rad] 

0"  0 

3.27  x  10~5  [rad] 

2.23  x  10~5  [rad] 

2.23  x  10-5  [rad] 

Unaided  Position  Std  Dev  For  Vertical  Drop 


Figure  4.5:  The  3-d  standard  deviation  of  the  position  states  in  the  unaided,  free,  INS  for 
the  vertical  drop 
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Aided  Position  Std  Dev  for  Vertical  Drop  Geometry  1 


Figure  4.6:  The  development  of  the  standard  deviation  of  the  position  errors  in  the  aided 
INS  for  geometry  1  for  the  vertical  drop. 


Aided  Position  Std  Dev  for  Vertical  Drop  Geometry  2 
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Figure  4.7:  The  development  of  the  standard  deviation  of  the  position  errors  in  the  aided 
INS  for  geometry  2  for  the  vertical  drop. 
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Table  4.3:  The  peak/final  standard  deviation  values  for 
the  unaided  navigation  states.  Also  included  are  the 
peak  and  final  standard  deviations  for  aided  navigation 
states  for  the  vertical  three-dimensional  case  using  the 
first  geometry  shown  in  Section  3.4. 


Standard  Deviation 

Unaided  Final  Value 

Aided  Peak  Value 

Aided  Final  Value 

crx 

35.4  [m] 

43.4  [cm] 

11.97  [cm] 

cry 

34.9  [m] 

43.0  [cm] 

11.70  [cm] 

<rz 

34.9  [m] 

11.4  [m] 

4.33  [m] 

<T  Vx 

.899  [m/s] 

.079  [m/s] 

.011  [m/s] 

cr  Vy 

.873  [m/s] 

.079  [m/s] 

.0029  [m/s] 

Vz 

.873  [m/s] 

.458  [m/s] 

.11  [m/s] 

0"  <p 

7.27  x  10~5  [rad] 

7.27  x  10-5  [rad] 

1.21  x  10~5  [rad] 

CTg 

7.27  x  1(T5  [rad] 

2.14  x  10~5  [rad] 

.937  x  10~5  [rad] 

0"  ip 

7.27  x  1CT5  [rad] 

1.21  x  10~5  [rad] 

1.21  x  10~5  [rad] 
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Table  4.4:  The  peak/final  standard  deviation  values  for 
the  unaided  navigation  states.  Also  included  are  the  peak 
and  final  standard  deviations  for  aided  navigation  states 
for  the  vertical  three-dimensional  case  using  the  second 
geometry  shown  in  Section  3.4. 


Standard  Deviation 

Unaided  Final  Value 

Aided  Peak  Value 

Aided  Final  Value 

crx 

35.4  [m] 

43.2  [cm] 

11.97  [cm] 

cry 

34.9  [m] 

56.1  [cm] 

16.55  [cm] 

<rz 

34.9  [m] 

.14  [mm] 

1.0365  [micron] 

<T  Vx 

.899  [m/ s] 

.079  [m/s] 

.011  [m/s] 

cr  Vy 

.873  [m/s] 

.091  [m/s] 

.0041  [m/s] 

Vz 

.873  [m/s] 

.0014  [m/s] 

25.91  [nm/s] 

0"  <p 

7.27  x  10~5  [rad] 

2.17  x  10~5  [rad] 

.929  x  10~5  [rad] 

CTg 

7.27  x  1(T5  [rad] 

2.17  x  10~5  [rad] 

.929  x  10~5  [rad] 

0"  ip 

7.27  x  1CT5  [rad] 

7.27  x  10~5  [rad] 

7.27  x  10~5  [rad] 
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5  Conclusion 


5.1  Conclusions 

Demonstrating  INS  aiding  action  using  vision  while  loitering  in  the  vicinity  of  a 
ground  feature  is  a  possibility,  but  then  one  does  not  go  places.  The  “bootstrapping” 
method  for  INS  aiding  during  cross  country  navigation,  that  is,  using  bearing 
measurements  of  ground  features  as  they  come  into  view,  shows  that  an  aircraft  can  use  its 
own  position  to  geolocate  a  ground  object,  then  use  that  geolocation  to  strongly  aid  its 
own  INS  and  repeat  the  process  as  long  as  necessary.  While  the  errors  in  the  horizontal 
position  will  continue  to  grow  unbounded,  the  rate  of  growth  was  cut  by  99.5%.  If  one 
considers  the  volume  of  position  uncertainty  for  the  unaided  INS  is  .7071  km 3,  from 
Eq.  (4.3),  and  contrasts  that  with  the  position  uncertainty  volume  of  the  optical  tracking 
aided  INS,  7.4177  x  10_1()  km3 ,  it  is  clearly  shown  that  despite  the  negative  connotation  of 
“bootstrapping”,  it  is  a  mechanism  that  makes  INS  aiding  using  vision  practical  for  long 
range  flight. 

It  is  also  clearly  shown  that  using  visual  bearings-only  measurements  greatly  reduces 
the  uncertainty  in  the  INS  provided  navigation  state  estimate  during  a  vertical  drop.  This 
improvement  will  allow  for  more  accurate  guidance  of  the  munition,  and  therefore  a 
greater  chance  of  the  munition  hitting  its  target.  This  is  achieved  using  a  passive  means 
and  autonomous  guidance. 

5.2  Follow-On  Topics 

There  are  still  many  ways  this  research  can  be  expanded  upon  to  take  it  from  the 
realm  of  the  academic  to  the  operational  realm.  Consider  the  following 

•  Perform  an  error  state  analysis.  Find  out  how  often  the  estimates  actually  fall  within 
lcr  of  the  truth  information. 
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•  Consider  a  maneuvering  A/C  where  the  A/C  Euler  angles  cannot  be  assumed  small. 
There  are  few  instances  where  a  military  A/C  will  be  able  to  fly  a  straight  course  to 
the  target.  The  A/C  will  need  to  avoid  densely  covered  air  defense  areas,  when 
possible.  This  dictates  a  maneuver,  and  sometimes,  multiple  drastic  maneuvers. 

•  Consider  a  long  range  munition  on  a  ballistic  terminal  trajectory.  This  would 
provide  an  effective  means  of  combining  the  horizontal  and  vertical  three 
dimensional  cases. 

•  Transition  from  a  flat,  non-rotating  Earth  to  spherical  rotating  Earth.  The  higher 
quality  navigation  systems  will  be  able  to  detect  the  rotation  of  the  Earth,  which  can 
introduce  further  errors  into  the  system. 

•  Close  the  guidance-control  loop  to  maneuver  the  munition  towards  the  target. 
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Appendix  A:  2-D  Standard  Deviation  Plots 


0  500  1000  1500  2000  2500  3000  3500  4000 


x  10~3 


Figure  A.l:  The  2-d  standard  deviation  of  the  velocity  states  in  the  unaided,  free  INS 
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x  10 
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2-d  Aided  Velocity  Uncertainty 


x  10 


Figure  A. 2:  The  2-d  standard  deviation  of  the  velocity  states  in  the  aided  INS 


2-d  Unaided  Attitude  Uncertainty 


Figure  A. 3:  The  2-d  standard  deviation  of  the  attitude  state  in  the  unaided,  free  INS 
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2-d  Aided  Attitude  Uncertainty 


Figure  A. 4:  The  2-d  standard  deviation  of  the  attitude  state  in  the  aided  INS 


2-d  Ground  Feature  Position  Uncertainty 


x  ICf3 


Figure  A. 5:  The  standard  deviation  of  the  tracked  ground  objects’  position,  over  1  hour 
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Ground  Object  Position  Uncertainty 


x  10 


Figure  A. 6:  The  standard  deviation  of  the  tracked  ground  objects’  position,  over  250 
seconds 
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Appendix  B:  3-D  Horizontal  Plots 


3-d  Unaided  Velocity  Uncertainty 


Figure  B.l:  The  3-d  standard  deviation  of  the  velocity  states  in  the  unaided,  free  INS 
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Figure  B.2:  The  3-d  standard  deviation  of  the  velocity  states  in  the  aided  INS 


3-d  Unaided  Attitude  Uncertainty 


Figure  B.3:  The  3-d  standard  deviation  of  the  attitude  state  in  the  unaided,  free  INS 
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3-d  Aided  Attitude  Uncertainty 


Figure  B.4:  The  3-d  standard  deviation  of  the  attitude  state  in  the  aided  INS 


3-d  Ground  Point  1  Position  Uncertainty 


Figure  B.5:  The  standard  deviation  of  the  closer  tracked  ground  object’s  position 
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3-d  Ground  Point  2  Position  Uncertainty 


Figure  B.6:  The  standard  deviation  of  the  further  tracked  ground  object’s  position 
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Appendix  C:  3-D  Vertical  Plots 


Unaided  Velocity  Std  Dev  For  Vertical  Drop 


Figure  C.  1 :  The  development  of  the  standard  deviation  of  the  velocity  errors  in  the  unaided, 
free  INS  for  the  vertical  drop. 
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Unaided  Angular  Std  Dev  For  Vertical  Drop 


Figure  C.2:  The  development  of  the  standard  deviation  of  the  angle  errors  in  the  unaided, 
free  INS  for  the  vertical  drop. 


Aided  Velocity  Std  Dev  for  Vertical  Drop  Geometry  1 


Figure  C.3:  The  development  of  the  standard  deviation  of  the  velocity  errors  in  the  aided 
INS  for  geometry  1  for  the  vertical  drop. 
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Aided  Angular  Std  Dev  for  Vertical  Drop  Geometry  1 


Figure  C.4:  The  development  of  the  standard  deviation  of  the  angle  errors  in  the  aided  INS 
for  geometry  1  for  the  vertical  drop. 


Std  Dev  Ground  Feature  1  Position  Geometry  1 


Figure  C.5:  The  development  of  the  standard  deviation  of  the  first  ground  object’s  position 
for  geometry  1  for  the  vertical  drop. 
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Std  Dev  Ground  Feature  2  Position  Geometry  1 


Time  (sec) 


Figure  C.6:  The  development  of  the  standard  deviation  of  the  second  ground  object’s 
position  for  geometry  1  for  the  vertical  drop. 


Aided  Velocity  Std  Dev  for  Vertical  Drop  Geometry  2 
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Figure  C.7:  The  development  of  the  standard  deviation  of  the  velocity  errors  in  the  aided 
INS  for  geometry  2  for  the  vertical  drop. 
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Aided  Angular  Std  Dev  for  Vertical  Drop  Geometry  2 


Figure  C.8:  The  development  of  the  standard  deviation  of  the  angle  errors  in  the  aided  INS 
for  geometry  2  for  the  vertical  drop. 


Std  Dev  Ground  Feature  1  Position  Geometry  2 


Figure  C.9:  The  development  of  the  standard  deviation  of  the  first  ground  object’s  position 
for  geometry  2  for  the  vertical  drop. 
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Std  Dev  Ground  Feature  2  Position  Geometry  2 


Time  (sec) 


Figure  C.10:  The  development  of  the  standard  deviation  of  the  second  ground  object’s 
position  for  geometry  2  for  the  vertical  drop. 
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Appendix  D:  General  Form  Dynamics 


The  following  is  the  derivation  of  the  state  error  equations  for  the  case  when  the 
Euler  angles  are  not  small.  It  will  allow  for  aircraft  maneuvers.  The  assumptions  about  a 
flat  and  non-rotating  Earth  are  still  used. 

The  DCM  relating  the  accelerometer  measurements  in  the  body  frame  to  the  specific 
forces  in  the  navigation  frame  [9]  is 


such  that 


C 


n  _ 

b 


cOcif/  s9  scp  cip  -  ccp  sip  ccp  s9  cip  +  scp  sip 
c9  sip  s9  scp  sip  +  ccp  cip  ccp  s9  sip  -  scp  cip 
-s  9  s  cpc9  c9ccp 


Fx, 

c9cip  s9  scp  cip  -  ccp  sip  ccp  s9  cip  +  scp  sip 

fxb 

Fy, 

= 

c9sip  s9  scp  sip  +  ccp  cip  ccp  s9  sip  -  scp  cip 

fyb 

FZi  _ 

-s  9  scpc9  c9ccp 

fzb 

(D.l) 


(D.2) 


F xi  =  (c 9 cip)fXb  +  (s 9  scp  of/  -  ccp  s >J/)fyb  +  (ccf)  s 9  ciJj  +  s (p  s ip)fZb  (D.3) 

Fyi  =  ( c9  s iff)fxb  +  (s 9  S(p  SI A  +  C(p  Ci/f) fyb  +  (c<p  s 9  si//  -  scp  ci//)fZb  (D.4) 

Fzl  =  (-  s  9)fXb  +  (s  </>c9)fyb  +  (c  9ccp)fZb  (D.5) 


However,  the  sensors  have  inherent  uncertainty  in  them.  As  such,  Eqs.  (D.3)-(D.5)  must 
be  perturbed  using  the  First  Order  Taylor  Series  Expansion  (FOTSE). 


5Fxi  =  6(/){{s9c(/)  ci//  +  s (p  si fi)fyb  +  (ccp  sip  -  scp  s9cip)fZb) 

+  59(-  s 9 ci p  fXb  +  c 9  scp cip  fyh  +  c9 ccp  cip  fZb) 

+  8ip(-  sip  c9  fXb  -  (s 9  scp  sip  +  c9  cip)fyb  +  (scp  cip  -  ccp  s 9  sip)fZb) 
+  8fXb(c9cip)  +  8fyb(s9scpcip  -  ccp  sip )  +  8fZb(ccp  s 9 cip  +  scp  sip ) 


(D.6) 
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and 


SFyi  =  8ep{{ s8  cep  sip  -  s ep  cip)fyb  -  (s 8  sep  si p  +  cep  cip)fZb) 

+  88{-  s 8  sip  fXb  +  c8  sep  si p  fy„  +  c8  cep  sip  fZb) 

+  8ip(cd  ci p  fXb  +  (s 8  sep  cip  -  cep  si p)fyb  +  (s 8  cep  ct p  +  sep  si p)fZb) 
+  8fXb(c6  sip )  +  8fyb(s6  sep  sip  +  cep  cip)  +  8fZb{cep  s 6  sip  -  sep  cip) 


(D.7) 


8Fzi  =  Sep(c6cepfyb  -  s epc6fZb)  +  88(-c6fXb  -  s9sepfyb  -  s6cepfZb) 

(D.8) 


-  8fXb(s6)  +  8fyb{sepc6)  +  8fZb{c6cep) 

are  the  results  of  the  FOTSE.  The  errors  in  the  specific  forces  are  related  to  the  error  states 
by 


8vx  =  8Fxi  Svy  =  8Fyi  8vz  =  8Fzi 

8x  =  8vx  8y  =  8vy  8z  =  8vz 


Due  to  the  overwhelming  complexity  of  the  dynamics,  the  A  matrix  will  be  assembled 
piecemeal.  If  a  component  of  the  matrix  is  not  listed,  it  has  a  value  of  zero. 


A(l:3,4:7)  =  I 


(D.9) 


A(4:6,7) 


(s 6  cep  cip  +  sep  sip)fyb  +  ( cep  sip  -  sep  s 0  cd)fZb 
(s 8  cep  sip  -  sep  cip)fyb  -  (s 8  sep  sip  +  cep  cip)fZb 
cdcepfyb  -  sep  cd  fZb 


A(4:6,  8) 


-  s 8  cip  fXb  +  c8  sep  cip  fyb  +  c8  cep  cip  fZb 
~  s 8  sip  fXb  +  c8  sep  sip  fyb  +  c8  cep  sip  fZb 
-c8fXb-s8sepfyb-s8cepfZb 


A(4:5,9) 


-  sip  c8  fXb  -  (s  8  sep  sip  +  c8  cip)fyh  +  ( sep  cip- cep  s8  si p)fZb 
c8  cip  fXb  +  (s 8  sep  cip  -  cep  sep)fyb  +  (s 8  cep  cip  +  sep  sip)fZb 


(D.10) 


(DU) 


(D.12) 
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The  disturbance  matrix,  T.  while  complicated,  is  still  simple  enough  that  it  can  be  listed  in 
its  entirety 


r  = 


o  o 

0  0 

0  0 


cOcip  (s6  scp  ci// -  ccp  si//) 
c6  si//  (c (p  c\j/  +  s 6  scp  si//) 
-s  6  scpcO 


0  0 
0  0 
0  0 


0 

0 

0 

(s0  si// +  sd  cep  ci//) 
(s6  si//  ccp  -  s/p  ci//) 
cOc/p 
0 
0 
0 


0  0 
0  0 
0  0 
0  0 
0  0 
0  0 
1  0 
0  1 
0  0 


(D.13) 
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Appendix  E:  General  Form  Measurement  Model 


E.l  Measurement  for  Primarily  Horizontal  Flight 


For  the  case  when  the  camera  is  located  on  the  underside  of  an  aircraft,  where  the 
focal  plane  is  orthogonal  to  the  negative  vertical  axis.  When  the  aircraft’s  Euler  angles 
cannot  be  assumed  small,  the  nonlinear  DCM  from  the  navigation  frame  to  the  body  frame 


c9ci// 


c9s  i// 


Cbn  -  s 9  s (p  ci fj  -  cep  si//  s 9  scp  si//  +  cep  ci//  s (p  c9 
ccp  s9  ci// +  scp  si//  ccp  s9  si]/ -  scp  ci//  c9ccp 
and  the  relationship  between  the  navigation  frame  and  the  body  frame  is 


xp  -  x 


yP-y 


Xp  -  X 


-n  I  yp-y 


Zn~Z 


The  angles  the  angle  from  the  x{h)  axis  to  the  tracked  ground  feature,  and  /u,  the  angle 
from  the  y(h)  axis  to  the  tracked  ground  feature,  provide  the  relationship  basis  for  the 


measurement  equations. 

Xf  ( Xp  -  x)(b) 

tan<f  =  —  = - — 

/  (zp  -  z)m 

y,  (yP-yt' 

,an//  =  j  =  -^—f) 

The  x  measurement  will  be  shown  first.  The  nondimensionalized  x/, 

cd  cip(xp  -  x)  +  c9  sij/(yp  —y)  +  s 9(z) 

Xf  —  - 

(c0  s 9  ci//  +  s0  s i//)(xp  -  x)  +  ( cep  s 9  si//  -  scp  ci]/)(yp  -  y)  -  c9  c/p{z) 
Moving  everything  to  the  LHS  of  the  equation  produces 

Xf{{c(p  s 9  ci]/  +  s cf)  s i//)(xp  -  x)  +  ( cep  s 9  si//  -  s <p  ci//)(yp  -  y)  -  c9ccp(z)) 
-  c9ci//(xp  -  x)  -  c9  si//(yp  —y)  —  s 9(z)  =  0 
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Due  to  the  sheer  complexity  of  the  equation,  it  is  broken  up  into  the  following  components 

A  =  ccp  s6  cip  +  scp  sip 

(E.7) 

B  =  ccp  s6sip-scpcip 

(E.8) 

C  =  cdc(p 

(E.9) 

D  =  cdcip 

(E.10) 

E  =  cd  sip 

(E.ll) 

ii 

■F 

i 

(E.12) 

1 

II 

(E.13) 

such  that 

(. Xfm-6xf)(AF  +  BG  +  Czc)  -  DF  -  EG  +  sdc  zc  =  0 

The  states  and  measurements  must  then  be  perturbed  to  account  for  uncertainty  from  the 
INS  and  measurements. 

6  =  6C  -  86  cp  -  <pc  -  6(f>  ip  =  ipc  -  Sip 

x  =  xc  -  Sx  y  =  yc-  Sy  z  =  zc  ~  Sz 

Xp  —  XpC  8xp  y  p  —  y  pC  8y  p  xj  —  xym  8xy 

Based  on  the  angle  addition  rule  and  the  small  angle  assumption  of  the  error  terms,  the 
trigonometric  terms  have  the  following  relationships 

sin(@c  -  86)  =  s 6C  -(s 6C  +  c6c)86  cos (8C  -  86)  =  c6c  +(s 6C  -  c8c)S6 

sin (cpc  -  8cp)  =  S(pc  ~(scpc  +  ccpc)S(p  cos ((pc  -  8cp)  =  ccpc  +(s (pc  -  ccpc)8cp 

sin(i/ec  -  Sip)  =  s ipc  -(s ipc  +  cp/c)8i l>  cos(ipc  -  Sip)  =  cipc  +(s ipc  -  cipc)8if/ 
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Substituting  the  perturbed  trigonometric  terms  into  the  component  terms  from 
Eqs.  (E.7)-(E.ll),  distributing  the  terms,  and  collecting  like  error  terms  yields 
A  =  ( c0c  +(s^c  -  c0c)<50)(  s9c  -(s 9C  +  c9c)69)(  apc  +(s</ic  -  ci//c)6i//) 

+  ( s0c  -(s0c  +  C(pc)S(p)(  si/^c  -(si/rc  +  aj/c)6i//) 

=  (  S 9C  C(pc  +(S0C  -  C(pc )  s9c  5(p  -  (s 9C  +  c9c )  C0c  d0)(  Cl/^  +(si/rc  -  Cipc)8ip) 

+  s^c  S(Ac  -  S(f)c(si//c  +  Cipc)8ip  -  Sl/^S^c  +  C0c)d0 

=  s@c  C(pc  cifjc  +  s0c  ci/rc(s0c  -  C(pc)8(p  -  c<pc  apc(s9c  +  c9c)S9  +  (si//c  -  ct/rc)  s@c  c0c  di/f 

+  S0C  S(Ac  -  S0c(s l/^c  +  Clpc)dlp  -  Sl/^S^c  +  C0c)d0 
A  =  (  S0C  C0c  Cl/rc  +  S(pc  S(/rc  )  -  d#(  C0c  Cl^c(s0c  +  C0c)) 

+  8(p(  s9c  C(/rc( S0C  -  C0c)  -  si/rc(s0c  +  C0c))  +  8l//((siffc  -  C(/rc)  S#c  C(pc  -  S(pc(siffc  +  Ct/rc)) 

(E.14) 


B  =  ( C(pc  +(s (pc  -  C(pc)8(p)(  s9c  -(s ec  +  c9c)S9)(  si//c  -(s ipc  +  ct/OdiA) 

-  (  S0C  -(s0c  +  C0c)d0)(  Cl/^c  +(s<Ac  -  Clf/c)8l//) 

=  ( s0c  C0c  -  C(pc(s9c  +  c9c)89  +  sdc(s(pc  -  C0c)d0)(  s<Ac  -(s^c  +  cif/c)Sif/) 

-  S0C  Cl/rc  -  S0c(s l]/c  -  Cljjc)8lp  +  Cl//c(S(f>c  +  C(pc)8(p 

=  s9c  si/cc  C(pc  -  s9c  ccf)c(sij/c  +  ctj/c)5i//  -  si//c  C(pc(s9c  +  cdc)86  +  s 6C  si//c(s(pc  -  C(pc)8(p 

-  S0C  Cl /rc  -  S0c(s lpc  -  Cljjc)8lp  +  Cl//c(S(f)c  +  C(pc)8(p 

B  =  ( s9c  sipc  C0c  -  S0C  C(/rc )  -  89(  si/^c  C0c(s0c  +  c9c )) 

+  d<A(  s6»c  S<Ac(s<Ac  -  C0c)  +  Cl//c(S(pc  +  C0c))  -  dlA(  S0C  C0c(si/lc  +  C^c)  +  S0c(si/rc  -  Cl/rc)) 

(E.15) 

C  =  ( cdc  +(s9c  -  C0c)d6>)(  C(pc  +{s(pc  -  C(pc)8(p ) 

(E.16) 

=  c0c  c0c  +  C0c(s0c  -  c4>c)8(p  +  c0c(s 0C  -  cdc)89 
D  =  (c9c  +(s9c  -  c9c)89)(  ci/rc  +(8^  -  cifrc)6if/) 

(E.17) 

=  c0c  ci/^c  +  c9c(sif/c  -  af/c)Sif/  +  ci/rc(s@c  -  c9c)89 
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and 


E  =  (si/fc  -(s0c  +  aJ/c)8t//)( c9c  +(s 0C  - C0c)<50) 

=  s</rc  c#c  +  si//c(s9c  -  c9c)89  -  c9c(si//c  +  af/c)Sif/ 

Notice  that  the  error  terms  that  were  multiplied  together  were  neglected.  The 
perturbations  were  then  substituted  into  Eqs.  (E.12)  and  (E.13)  producing 


(E.18) 


F  =  (xpc-xc)  -  8xp  +  Sx  (E.19) 

and 

G  =  (ypc-yc)  +  8y  -  8yp  (E.20) 

Next,  we  started  to  merge  the  components  by  distributing  their  terms,  again  whenever 
error  terms  were  multiplied  together,  they  were  neglected  due  to  how  small  they  are. 

AF  =  ( s 6C  c <pc  cif/c  +  s (pc  si//c  ){xpc-xc)  +  8x(  s dc  C(pc  aj/c  +  s0c  s if/c ) 

-  89(  C(f)c  ciffc(s9c  +  c6cj)(xpc-xc) 

+  Sif/(( sif/c  -  cipc)  s 9C  C(pc  -  s0c(s<Ac  +  apc))(xpc-xc) 

+  8(f){  S 9C  Cl/rc(s0c  -  C(f>c)  -  S(/rc(s0c  +  C(f>c))(xpc-xc )  -  8xp(  S 9C  C(pc  Clf/c  +  S(pc  Slffc  ) 

(E.21) 


BG  =  ( s9c  s tffc  C(f)c  -  s0c  aAc  )(yPc-yc)  ~  89(  s if/c  c0c(s 9C  +  c@c))(»c-yc) 

+  8<p(  s 9C  si^c(s0c  -  C0c)  +  Ci/rc(s0c  +  C(pc))(ypc-yc ) 

(E.22) 

-  8ip{  s 9C  c0c(s<Ac  +  aft 'c)  +  s0c(s^c  -  cipc))(ypc-yc) 

+  b}’(  S0C  S(/rc  C0c  -  S0C  C(/rc  )  -  (5>’p(  S0C  S^c  C(pc  -  S0C  Cl/fc  ) 

Cz  =  —(c9c  C(pc  zc  +  8(p  c9c( scpc  -  C(pc)zc  +  89cc[)c(s9c  -  c9c)zc  -  8zc9c  ccpc)  (E.23) 
The  merged  terms  are  collected,  such  that 

Q=AF  +  BG  +  Cz  (E.24) 
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and  the  result  Q  is  then  multiplied  by  the  measurement  term. 


XfQ  —  (*^/m  s#c  C0c  Cl//c  +  S0C  Slf/C  ^)(XpC  %c) 

+  ( s 9C  s<Ac  c0c  -  s0c  apc  )(ypc-yc)  -  c9c  ccpc  zc| 

+  8x(  s6c  C(pc  cipc  +  s0c  sipc  )xf  +  8y(  s 9C  s ipc  c^e  ~  s (pc  cipc  )x/  +  8z  c 9C  c <pc  Xf 

-  89^(  ccpc  cipc(s 9C  +  c9c))(xpc-xc )  +  s<Ac  C0c(s0c  +  c9c)(ypc-yc )  +  c^s^  -  C0c)zc  W 
+  d<^(  S0C  Clpc(S(pc  -  C0c)  -  Slpc(S(pc  +  C(pc))(xpc~Xc )  -  c9c(S(pc  -  C(f)c)zc 

+  (  S0C  S(Ac(S0c  -  C^c)  +  Cl/rc(s0c  +  C(pc)){y  pc-yc) yf 
+  (^((si^c  -  Cl/rc)  S0C  C0c  -  S(pc(sipc  +  apc))(xpc-xc) 

-  ( s0c  cpc(sipc  +  ClAc)  +  S0C(S0-C  -  Cipc))(ypc-yc)jxf 

-  (5.rp(  s0c  c^c  ci/^c  +  S(pc  sipc  )jc/  -  dyp(  s 9C  sipc  C(pc  -  scpc  cipc  )xf 


the  remainder  of  the  components  were  merged 

—DF  =  -  c9c  cipc(xpc-xc)  -  Sxc9c  apc  -89cipc(s9c  -  c9c)(xpc-xc) 
Sip  c9c(sipc  cip  ^ ) ( xpi:  .v( )  +  Sxp  c9c  c ip q 

-EG  =  -  sipc  c9c(ypc-yr)  -  Sy  s ipc  c9c  -89  sipc(s9c  -  c9c)(ypc-yc) 

+  8ip  c9c(sipc  +  c ipc)(ypc~yc)  +  8yp  sipc  c9c 

z  s9  =  -  s  9C  zc  +  89(s9c  +  c9c)zc  +  8z  s  9C 


(E.25) 


(E.26) 

(E.27) 

(E.28) 


and  collected,  such  that 
O  -  -DF  -  EG  +  zs9 

=  -  c9c  cipc(xpc-xc )  -  sipc  c9c(ypc-yc )  -  s 9C  zc  -  8x  c9c  cipc  -8y  s ipc  c9c  +8z  s 9C 
89((s9c  +  c9P)Zc  "t-  cipc(s9c  c9P)(xpc  xP)  +  sipc(s9c  c9P)(y pc  y^)) 

88/ ( c9c(sipc  cipP)(xpc  xp)  c9c(sipc  cip ppy pc  y^))  8xp  c$c  ci pc  ~\~8y p  s ipc  c9^ 

(E.29) 
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Moving  all  of  the  error  terms  to  the  RHS  of  the  equation  yields  the  x  measurement 
equation  for  the  primarily  horizontal  case 


S0C  C0c  )(-^/7c  %c)  C0c  S0c  C(^c  ^)(y pc  yc )  C0c  Zc 

C@c  -Vc)  S^c  £@c(y pc  ^Vc)  S$c  C@c  CiJ/q  (  S$c  C0c  CiJ/q  S (j)^  S lf/c 

-  5v|(  s 9C  s ipc  ccp c  -  s0c  cip c  )x/  -  st/rc  c0c  j  -  <fe(  s0c  +  cdc  ccpc  x/) 

+  50  ( Cipc(s9c  -  C0c)(xpc-xc)  +  sipc(s9c  -  C0c)(ypc-yc)  -  (s0c  +  c9c)zc)+ 

C(Ac(S0c  "t"  C0c))(A/>c  *c)  Sl/fc  C0c(S0c  ^@c)(y pc  Tc)  ^Cpc(s9c  c9c')Zc^Xf 

-  50U  S0C  Clpc(scpc  -  C0c)  -  Slpc(scpc  +  CCpc))(xpc~Xc )  -  C0c(s0c  -  c</>c)zc 
+  ( s0c  sipc(scpc  ccpP)  +  Ci/rc(s0c  +  ccpc))(ypc  yc)  W 

+  (5i/^  cOc(sif/c  c^c)(XpC  xc)  cOc(si//c  +  ci//c)(y pC  y<?) 

(((S<Ac  C^c)  S0C  C0c  S0c(S(/fc  H"  C(^c))(XpC  xc) 

-  ( s0c  ccpc(sipc  +  cipc)  +  scpc(sipc  -  cipc))(ypc-yc)jXf 

+  8xp[(  s9c  C0c  Cfc  +  S0C  S^c  )x/  -  C0c  Cl^c  j  -  5y  J  Sl/^c  C0c  -(  S0C  S(/rc  C0c  -  S0C  Cl^c  )x/J 
+  8Xf^(  S0C  C(/>c  ClAc  +  S(/>c  S0"c  ^)(xpc  -C)  “t"  (  S0C  C0c  S0C  ClAc  )Cy^c  Tc)  C0c  C0c 

(E.30) 


where  x/  is  determined  by  plugging  the  nominal  values  for  the  A/C  Euler  angles  into 
Eq.  (E.5). 

The  process  then  had  to  be  repeated  for  the  y  measurement. 

(xp  -  x)(s9  s cp  cip  -  ccp  sip)  +  (yp  -  y)(s9  s cp  sip  +  ccp  cip)  -  z(scp  c6) 

y  f  —  -  (. 

(ccp  s 6  ci p  +  s cp  sip)(xp  -  x)  +  ( ccp  s 6  sip  -  scp  cip)(yp  -  y)  -  c9  ccp(z) 

Moving  everything  to  the  LHS  of  the  equation 


y /((ccp  s 9  cip  +  scp  s ip)(xp  -  x)  +  (ccp  s 9  sip  -  scp  cip)(yp  -  y)  -  c9  ccp(z)) 

-  (xp  -  x)(s 9  scp  cip  -  ccp  sip )  -  (yp  -  y)(s9  scp  sip  +  ccp  cip)  +  z(scp  c9)  =  0 


(E.32) 
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Since  the  denominator  in  Eq.  (E.31)  is  the  same  as  in  Eq.  (E.5),  many  of  the  components 
are  the  same  as  in  the  x  measurement.  The  unique  components  are 

I  =  S0  $(f)  Cl//  —  C(f)  sif/ 

J  =  s0  s0  sif/  +  ccp  of/ 

L  =  sficO 

Substituting  in  the  perturbations,  distributing  the  terms,  and  collecting  like  error  terms 
yields 

/  =  ( s0c  — (s0c  +  C0c)d0)(  s0c  -(s (pc  +  C0c)d0)(  aj/c  +(s if/c  -  c i/fc)6i/f) 

-  (  C0c  +(s (pc  -  C0c)d0)(  S(/rc  -(s lf/c  +  C l/fc)6l/f) 

=  ( s0c  S0C  -  s0c(s^c  +  C(pc)6(p  -  S0C(S0C  +  cdc)S6)(  af/Q  +(s^c  -  cif/c)5if/) 

-  sipc  C(pc  +  C^c(s^c  +  Ctf/c)6iff  -  si^c(s0c  -  C(pc)6(p 

=  s0c  s0c  cif/c  +  s0c  s0c(s if/c  -  af/c)di//  -  s0c  ct/rc(s (pc  +  cep c)(50  -  s0c  cij/c(sdc  +  cdc)66 

-  sipc  C(pc  +  C^c(s^c  +  Cifjc)5^  -  si^c(s0c  -  C0c)d0 
/  =  (S0C  S0C  C(/rc  -  S lf/c  C0c)  -  (50  S0C  C</(c(s0c  +  C0c) 

-  6cf>(  si/^c(s0c  -  C0c)  +  s0c  Cij/c(s(pc  +  c <f>c))  +  6iff{  c<pc(sif/c  +  ct/O  +  s0c  s<pc(sif/c  -  ci//c)) 

(E.33) 
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J  =  (  S0C  -(s0c  +  C0c)(50)(  S0C  -(s^c  +  C0c)50)(  si/cc  — (si^c  +  Cl//c)6tJ/) 

+  ( C0c  +(S0C  -  C(pc)8(p)(  Cijjc  +(si/rc  -  ci//c)8if/) 

=  ( s0c  S0C  -  s0c(s^c  +  C0c)£0  -  s0c(s0c  +  c6»c)(5@)(  s</cc  -(st/rc  +  ajjc)8ifj) 

+  C0c  Ci/rc  +  C^c(si/rc  -  a]/c)6ip  +  Ci/rc(s0c  -  C(j)c)8(p 

=  s0c  s0c  st/cc  -  s0c  s0c(s i/cc  +  ciJ/c)SiJ/  -  s0c  si/^c(s^c  +  C(pc)8(p  -  scf)c  st/cc(s0c  +  cdc)88 
+  c (pc  cif/c  +  C(pc(siffc  -  cij/c)8if/  +  Ci/rc(s 4>c  -  C(pc)6(f) 

J  =  (s 8C  S0C  si/rc  +  C(pc  ci/O  -  88(  S(pc  s<Ac(s6»c  +  c0c)) 

-  (50(  s ec  S</(C(S0C  +  C0c)  -  ClJ/c(S(pc  -  C(pc ))  +  Sl//(  C0c(S(Ac  -  CtAc)  -  s0c  S0c(s i/rc  +  Cl/O) 


and 


(E.34) 


L  =  (scpc  ~(s(f>c  +  C(pc)8(p)(  c9c  +(s 9C  -  c 8C)68) 

=  s0c  c8c  +  s0c(s0c  -  c8c)88  -  c8c(scpc  +  C(pc)8(p 
Merging  the  noncommon  components  yields 


(E.35) 


IF  —  (s0c  s (pQ  ciAc  si/fc  C(F )(x pc  x()  “t-  (50  s0c  cipc(s9c  -I-  c0<- ^(x  pr  x^) 

"t”  8(J)(  S<AC(S0C  C(pc)  +  S0C  Clf/c(S(f)c  "t"  C(pc')')(,Xpc  -Tc) 

-  (5</((  C0C(SI^C  +  Cl/rc)  +  S0C  S0c(S</cc  ~  Cl//c))(xpc-Xc) 

(5x(s0c  S0c  C(/rc  Sl/^c  C(J)q)  +  SXp(s9c  SCpc  dfrc  S0c 


(E.36) 


-JG  =  -(s0c  s^c  si/rc  +  c0c  Cif/c)(ypc-yc)  +  88(  s0c  s</cc(s0c  +  C0c))(}y-yc) 


+  (50(  S0C  s<Ac(s0c  +  C0c)  -  Ci/rc(s0c  -  C(pc))(ypc-yc) 


-  6lJ/(  C0c(s (Ac  -  CtAc)  -  s0c  S0c(si/rc  +  ci[/c))(y  pc-yc) 


(E.37) 


and 


-  0y(s0c  s^c  si/rc  +  c0c  ci/cc)  +  8yp(s8c  s0c  si/^c  +  c0c  ci//c) 


-  Lz  =  s^c  c0c  zc  +  s0c(s0c  -  c8c)zc  ~  5<P  C0c(s0c  +  C(t>c)zc  ~  s0c  c0c  &  (E.38) 
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Collecting  the  merged  components 


R  =  —IF  -  JG  -  Lz 

-  (s$c  S0c  Clf/c  Slf/C  C(po)(^XpC  Xc)  (s@c  S(pc  S0c  "t"  C0c  Cljy- )(v^(  yc )  S0C  C0c 

-  6x(s 9C  S0C  C(Ac  -  StAc  C0c)  -  5y(s0c  S0C  SlAc  +  C0c  C(Ac)  -  Sz  S<Ac  C0c 

+  66^  s0c  ciAc(s0c  +  c9c)(xpc—xc)  +  ( s0c  S(Ac(s0c  +  c9c))(ypc-yc )  +  s0c(s6>c  -  C0c)zc 
+  50  (  S0C(S0C  -  C0c)  +  S0C  ClAc(S0c  +  C(pc))(xpc-xc ) 

+  ( s#c  S0C(S0C  +  C0c)  -  ctAc(s0c  -  C(f>c))(ypc-yc)  -  C0c(s0c  +  C0c)zc 

-  50^(  C0c( StAc  +  C0c)  +  s#c  s0c(stAc  -  Cijjc))(xpc-xc) 

+  ( C0 c(siAc  -  C0c)  -  s#c  s0c(siAc  +  Cif/c))(ypc—yc) 


SXp(sOc  S0c  CfAc  S(Ac  p(sO c  S 0c  S<Ac  C0c  C(Ac) 


(E.39) 


The  common  terms,  represented  in  the  2  need  to  be  multiplied  by  the  y  measurement  term 


y fQ  ~  ( y fm  S 6C  C (f)c  Cl// c  +  S0C  S^Ac  )(«^pc  *c) 

+  (  S0C  StAc  C0c  -  S0C  CtAc  )(yPc~yc)  -  c9c  C0c  zc  j 

+  6x(  s9c  C0c  ctAc  +  S0C  StAc  )>’/  +  5y(  s 9C  sif/c  c0c  -  s0c  c0c  )>’/  +  <fe  c#c  C0c  A/ 

-  56^(  C0c  C0c(s0c  +  C 6c))(Xpc-xc)  +  S0c  C0c(s0c  +  c0c)Oy-yc )  +  ct^Cs^  -  c0c)ZcW 
+  50^(  S0C  ClAc(S0c  -  C0c)  -  SlAc(S0c  +  C(f>c))(xpc-Xc)  ~  C9c( S0C  “  C0c)zc 

+  ( S0c  S0c(s0c  -  C0c)  +  ctAc(s0c  +  C(pc))(ypc-yc)  W 
"t"  50^((S0c  CtAc)  S0C  C0c  S0c(StAc  "t"  C0c))(-^pc  -^c) 

-  ( s0c  c0c(stAc  +  ctAc)  +  s0c(stAc  -  ci/fc))(ypc-yc)jyf 

-  5.rp(  S0C  C0c  C0c  +  S0C  StAc  )>’/  -  5yp(  s#c  s<Ac  c0c  -  s0c  ctAc  )y/ 

(E.40) 
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Merging  R  and  Q ,  and  moving  all  the  error  terms  to  the  RHS,  produces  the  y  measurement 
equation  for  primarily  horizontal  case 


y  fm ^ C0c  C lf/c  S0C  S^C  -^c)  S0C  C^Ac  )(j^?c  3^c) 

(s^c  S(jAc  ^0c)(-^/7C  -^c)  S0C  S^Ac  C0c  ^^Ac)CVpc  3^c)  Zc  — 

<5.r|  s0c  s0c  c^c  -  s^c  c0c  -( s0c  c0c  c<Ac  +  s0c  s<Ac  )yfj 

-  <5>|(  S0C  S(/rc  C(pc  -  S0C  ci/rc  )>’/  -  S0C  S0C  S(/rc  -  C(pc  ClAc  j  -  <fe(  (  C0c  C(pc  yf)  -  S(pc  c9c  j 
+  C0c  C(/rc( s6c  +  c6c))(xpc-xc)  +  S(/rc  c<Ac(s0c  +  c6c)(ypc-yc)  +  C(pc(sdc  -  c6c)z)jyf 

-  (  S(pc  c<Ac(s0c  +  c Oc)(xpc-xc)  +  (  S(pc  sij/c(s6c  +  c 6c))(ypc-yc)  +  s^c(s0c  -  cdc)z , 


-  6(p 


+ 


|(  s 6C  Clj/c{y>(pc  -  C(pc)  -  Sl/rc(s0c  +  C<pc))(xpc-xc )  -  C0c(s0c  -  C0c)zc 

( s ec  si/rc(s0c  -  C0c)  +  ci//c(scf)c  +  c<Ac))0y-yc)W 
+  (  S(/rc(s0c  -  C</>c)  +  S0C  Cl^c(s0c  +  C(pc))(xpc-xc ) 

+  ( s0c  si/rc(s0c  +  C0c)  -  ci^c(s0c  -  C<pc))(ypc-yc )  -  C0c(s0c  +  C0c)zc 
4"  dlA  C<Ac(SlAc  4~  ClAc)  "t"  St?c  S(Ac(SlAc  ClAc ))(A/>c  -Vc) 

+  (  C^c(SlAc  -  ClAc)  -  S0C  S<Ac(SlAc  +  ClAc))(»c->c)j 
^((siAc  ClAc)  S$c  C0c  S0c(s<Ac  +  C<Ac))(.T^c  Xc) 

-  ( s0c  c^>c(siAc  +  ciAc)  +  s<Ac(siAc  -  c<Ac))0y-yc))};/ 

+  Arp|(  Sf9c  C0c  ClAc  +  S<Ac  SlAc  )>’/  -  (s#c  S0c  ClAc  “  SlAc  C0c)j 

-  <5}’p|(  s6»c  S<Ac  SlAc  +  C0c  ClAc  )  -  (  S#c  SlAc  C0c  “  S<Ac  ClAc  )>’/) 

+  Sv,((  S0C  C(f) c  ClAc  +  S0c  SlAc  )(^pC-A:c)  +  (  S0C  SlAc  C0c  -  S<Ac  ClAc  )(ypc-);c)  -  C#c  C0c  Zcj 

(E.41) 


where  y/  is  found  by  substituting  the  nominal  values  for  the  A/C  Euler  angles  into 
Eq.  (E.31). 
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E.2  Measurement  for  Primarily  Vertical  Flight 


For  the  munitions  case  where  the  camera  is  assumed  in  the  nose  of  the  munition  with 
the  focal  plane  orthogonal  to  the  longitudinal  axis.  The  new  measurements  will  be  in  the 
zu,}  and  ylh>  frame  of  reference 


tan£  = 


Zf 

f 


(zP~zYb) 

(xp  -  xYb> 


(E.42) 


where  z.f  is  the  projection  z(h)  coordinate  of  the  ground  feature  onto  the  camera  focal 
plane.  Since  z,/  equals  the  reciprocal  xj  from  Eq.  (E.5),  all  of  the  component  terms  are  the 
same.  All  that  needs  to  be  done  for  this  measurement  is  change  the  values  in  Eq.  (E.30) 
that  are  multiplied  by  the  measurement  term.  If  a  term  was  previously  multiplied  by  Xf  it 
will  not  be  multiplied  by  z.f,  but  if  it  was  not,  then  it  will  be  multiplied  by  z.f.  Also,  the 
sign  previously  connected  with  the  measurement  term  accompanies  the  transfer  of  the 
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measurement  term.  Therefore  the  z  measurement  equation 


C@c  C^AcC^pc  Xc)  C@c(y pc  3^c)  SdcZc^~^ 

(  S^c  C^c  S0c  S^Ac  )C^pc  ^c)  (  S^c  S^Ac  S0C  C<Ac  )(^Vpc  ^Vc)  Zc  — 

<kr|(  S0C  C0c  Ci/fc  +  S0C  siAc )  -  c9c  Cipc  z/j  -  by|  s ij/c  c9c  z/  -  ( s 9C  s i//c  ccf>c  -  s 4>c  aj/c )  j 

+  6z,(  s 9C  Zf  +  c 9C  C(pc )  +  69  ((s 9C  +  c9c)zc  +  Ci//c(s6c  -  c9c)(xpc-xc )  +  s^c(s 9C  -  c9c)(ypc-yc))zf+ 

( c<pc  ci//c(s 9C  +  c9c))(xpc-xc )  +  si/rc  C0c(s0c  +  c9c)(ypc-yc)  -  ccpc(s9c  -  c9c)zc 
-  6(p[{  S 9C  Cl//c(S(/)c  -  C(f)c )  -  si/rc(s0c  +  C0c)) (xpc~Xc)  +  C0c(s0c  -  C(/)c)zc 
+  (  s0c  si//c(sc/>c  -  C(f)c )  +  cif/c(s<f>c  +  ccf>c))(ypc-yc)  j 

+  (((st/rc  ClAc)  S0C  C0c  S0c(s0c  ~f~  Ct^c))(XpC  Xc)  (  S0C  C0c(s0'c  +  C(Ac)  “I"  S0c(S(Ac  c))(y pc  3k) 

( c@c(s^c  c(^c)(XpC  xc)  c#c(s<Ac  +  cif/c)(y pc  yc))z/ 

“h  C$c  CiJ/q  Zf  (  S^c  C0c  “1“  S0c  S$c  S^Ac  ^0c  S0c  C(Ac  )  S^Ac 


+  Szf(c9c  a//c(xpc-xc)  +  sif/c  c9c(ypc-yc)  -  s9c  zcj 


(E.43) 


where  Zf  is  found  by  substituting  the  nominal  values  for  the  A/C  Euler  angles  into  the 
reciprocal  of  the  RHS  of  Eq.  (E.5). 

The  y  measurement  based  on  /  being  along  the  longitudinal  axis  will  take  more  work 

,  _  yXf  _  ( yP  -  y)(b) 

“"= /=(*,-*>» 

(s 9  s</>  ci//  -  c<p  s i//)(xp  -  jc)(,!)  +  (s 9  s0  si//  +  cc/>  ci//)(yp  -  y)(n)  +  scp  c9(zp  -  z){n) 
c9cif/(xp  -  x)(n)  +  c9  s \f/(yp  -  y)(,l)  -  s 9{z„  -  z){n) 
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where  yxf  is  the  y  measurement  term  for  the  vertical  case.  Breaking  Eq.  (E.44)  into 
previously  defined  components 

yxf(DF  +  EG  -  s  6  z) 

—  (yxf  Syxf)(c0c  cipc(xpc  xc)  +  Sx  c0c  cipc  +80  cip  c(s0c  C0c)(xpc  x(:) 

+  Sip  C0c(sipc  cipc )(xpr  xc )  Sx j,  c0c  cipc  +  st^c  c0c(yPc  y(  ) 

+  Sy  s ipc  c0c  +60  sipc(s0c  -  C0c)(ypc-yc )  -  dip  C0c(sipc  +  Cipc)(ypc-yc)  -  6yp  s ipc  c0c 

-  S 0C  Zc  +  80(9,0 c  +  C0c)zc  +  6z  S0C) 

(E.45) 

—  (yxf  8yxf)(c0c  cipc(xpc  xr)  +  s ipc  C0c(ypc  yc)  s0c  Zc )  +  6x(c0c  cipc  yxf') 

+  6y(sipc  C0c  yxf)  +  6z( s0c  yxf ) 

+  60(cipc(s0c  C0c)(xpc  xc)  +  sipc(s0c  C0c)(ypc  yc)  +  (s0c  c0c)Zc)y  xf 

+  6ip(c0c(sipc  cipc)(xpc  xc)  C0c(sipc  +  cipc )(ypc  yc))yxf 

-  6xp( c0c  cipc  yxf  )  -  8yp(sipc  C0c  yxf  ) 
and 

R  =  -IF  -JG-LZ 

=  -(S0C  S0c  cipc  -  siAc  C<pc)(xpc-xc)  -  (s0c  s (pc  s ipc  +  C(pc  cipc)(ypc-yc)  -  s (pc  C0c  zc 

-  6x( s0c  S(pc  cipc  -  s|Ac  C(pc )  -  Sy( s0c  s0c  s ipc  +  ccpc  c ipc)  +  6z  s cpc  c0c 

+  80^  s (pc  Cipc(s0c  +  C0c)(xpc-xc )  +  ( s (pc  sipc(s0c  +  C0c))(ypc-yc)  -  s0c(s0c  -  c0c)zA 
+  8(f)  (  Slpc(S(f>c  -  C(pc )  +  S0C  Clpc(s<pc  +  C(pc))(xpc-xc) 

+  (  S0C  Slpc(S(f>c  +  C(pc)  -  Clpc(S(f>c  -  C(pc))(yPc-yC )  +  c0c(s (pc  +  C(pc)zc 

-  8lp[(  C0c(s Ipc  -  Cipc)  -  S0c  S0c(stAc  +  Cipc))(ypc-yc) 

+  (  C(pc(sipc  "t"  Cipc)  "t"  S0C  S(f>c(sipc  Clp c))(xpc  Xc 

+  8xp( S0c  S (pc  Cipc  ^lp c  CCpc)  "t"  Sy p(s0c  S (pc  Slpc  +  C(pc  Cipc) 

(E.46) 
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Finally,  collecting  the  like  terms  and  moving  the  error  terms  to  the  RHS,  yields  the  y 
measurement  equation  for  the  primarily  vertical  case 

yXf(c6c  a//c(xpc-xc)  +  st/rc  c 9c(ypc-yc)  -  sdc  zc) 


(sdc  S(/)c  Cl//c  S if/ (_■  C(/c )(x p(:  X()  (s@c  S (pQ  S \jj q  “I"  C(pc  ClJ/ c^){y pc  Vc )  S0C  cdc  Zc  — 


6x(sdc  s0c  ci//c  -  stAc  c</>c  -  cdc  ci//c  yxf)  +  Sy(sdc  s0c  s if/c  +  c0c  cipc  -  si//c  c6c  yxf) 


3z(s0q  y xf  "t-  s (pc  cdc)  b//[  ( si// scC  c/pc)  +  s Oq  ci//c(  sc/y  c(/)x ))(-^  pc  v( ) 


+  ( s 6C  S(Ac(s0c  +  C(pc)  -  ci//c(s(f>c  -  C(f>c))(ypc-yc)+ 


cOc(s(pc  +  C0c)^c]  S0[  scpc  ci//c(sOc  "t-  cOc)(xpc  xc )  "I-  ( s^c  si//c(sOc  "i-  cdc))(ypc  yc ) 


cOc)z.c  +  (ci//c(s0c  c9c )(x pC  Xc)  +  si/^c(s0c  c0c)(y  pc  yc)  ^  (s@c  C0c)£c)y.t/] 


+  d<A[(  C0c(s(/rc  -  Ci/rc)  -  S0C  S0c(si/rc  +  ci//c))(ypc—yc) 
+  ( C0C(S l//c  +  Clf/c)  +  s 0C  sc/)c(si//c  -  ci//c))(xpc-xc) 


-  (c0c(sif/c  -  cif/c)(xpc-xc)  -  c0c(s i//c  +  Ci//c)(ypc—yc))yxf 


+  6xp(c0 c  ci// c  yx/  -  (s0c  s0c  c^c  -  si/rc  c</>c))  +  Syp(si//C  c6c  yxf  -  (s 0C  s0c  si/^c  +  ccpc  ci//c)) 


+  6yxf(c6c  ci//c(xpc-xc)  +  si//c  C0c(jy-yc)  -  s6»c  zc) 


(E.47) 


where  yx/  is  found  by  substituting  the  nominal  values  for  the  A/C  Euler  angles  into 
Eq.  (E.44). 
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